#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <signal.h>
#include <math.h>
#include <iostream>
#include "hal/hal.h"     /* access to HAL functions/definitions */
#include "rtapi/rtapi.h" /* rtapi_print_msg */
#include "rcs.hh"
#include "posemath.h"       // PM_POSE, TO_RAD
#include "nml_intf/zuc.hh"  // ZUC NML
#include "nml_intf/zuc_nml.hh"
#include "nml_intf/zucglb.h"  // ZUC_NMLFILE, TRAJ_MAX_VELOCITY, etc.
#include "nml_intf/zuccfg.h"  // DEFAULT_TRAJ_MAX_VELOCITY
#include "inifile.hh"         // INIFILE
#include "rcs_print.hh"
#include "nml_oi.hh"
#include "timer.hh"
#include "zuclog.h"
#include "errcode.h"

/* Using halui: see the man page */

static int axis_mask = 0;
#define JOGJOINT 1
#define JOGTELEOP 0

#define MDI_MAX 64
#define SAFETY_DI_ON 2
#define SAFETY_DI_OFF 1
// clang-format off
#define HAL_FIELDS                                                                                                                     \
    FIELD(hal_bit_t, machine_on)         /* pin for setting machine On */                                                              \
    FIELD(hal_bit_t, bp_enable)                 /* pin for bp robot enable */                                                          \
    FIELD(hal_bit_t, machine_off)        /* pin for setting machine Off */                                                             \
    FIELD(hal_bit_t, machine_is_on)      /* pin for machine is On/Off */                                                               \
    FIELD(hal_bit_t, estop_activate)     /* pin for activating ZUC ESTOP  */                                                           \
    FIELD(hal_bit_t, estop_reset)        /* pin for resetting ESTOP */                                                                 \
    FIELD(hal_bit_t, estop_is_activated) /* pin for status ESTOP is activated */                                                       \
                                                                                                                                       \
    FIELD(hal_bit_t, mode_manual)         /* pin for requesting manual mode */                                                         \
    FIELD(hal_bit_t, mode_is_manual)      /* pin for manual mode is on */                                                              \
    FIELD(hal_bit_t, mode_auto)           /* pin for requesting auto mode */                                                           \
    FIELD(hal_bit_t, mode_is_auto)        /* pin for auto mode is on */                                                                \
    FIELD(hal_bit_t, mode_mdi)            /* pin for requesting mdi mode */                                                            \
    FIELD(hal_bit_t, mode_is_mdi)         /* pin for mdi mode is on */                                                                 \
    FIELD(hal_bit_t, mode_drag)           /* pin for requesting drag mode*/                                                            \
    FIELD(hal_bit_t, mode_is_drag)        /* pin for drag mode is on */                                                                \
    FIELD(hal_bit_t, mode_teleop)         /* pin for requesting teleop mode */                                                         \
    FIELD(hal_bit_t, mode_is_teleop)      /* pin for teleop mode is on */                                                              \
    FIELD(hal_bit_t, mode_teleop_tool)    /* pin for requesting teleop_tool mode */                                                    \
    FIELD(hal_bit_t, mode_is_teleop_tool) /* pin for teleop_tool mode is on */                                                         \
    FIELD(hal_bit_t, mode_joint)          /* pin for requesting joint mode */                                                          \
    FIELD(hal_bit_t, mode_is_joint)       /* pin for joint mode is on */                                                               \
                                                                                                                                       \
    FIELD(hal_bit_t, mist_on)     /* pin for starting mist */                                                                          \
    FIELD(hal_bit_t, mist_off)    /* pin for stoping mist */                                                                           \
    FIELD(hal_bit_t, mist_is_on)  /* pin for mist is on */                                                                             \
    FIELD(hal_bit_t, flood_on)    /* pin for starting flood */                                                                         \
    FIELD(hal_bit_t, flood_off)   /* pin for stoping flood */                                                                          \
    FIELD(hal_bit_t, flood_is_on) /* pin for flood is on */                                                                            \
    FIELD(hal_bit_t, lube_on)     /* pin for starting lube */                                                                          \
    FIELD(hal_bit_t, lube_off)    /* pin for stoping lube */                                                                           \
    FIELD(hal_bit_t, lube_is_on)  /* pin for lube is on */                                                                             \
                                                                                                                                       \
    FIELD(hal_bit_t, program_is_idle)                 /* pin for notifying user that program is idle  */                               \
    FIELD(hal_bit_t, program_is_running)              /* pin for notifying user that program is running */                             \
    FIELD(hal_bit_t, stepping_is_running)              /* pin for notifying user that program is running */                             \
    FIELD(hal_bit_t, program_is_paused)               /* pin for notifying user that program is paused */                              \
    FIELD(hal_bit_t, robot_is_poweron)                /* pin for notifying user that robot is power on */                              \
    FIELD(hal_bit_t, robot_is_enabled)                /* pin for notifying user that robot is enabled */                               \
    FIELD(hal_bit_t, robot_is_motion)                 /* pin for notifying user that robot is in motion */                             \
    FIELD(hal_bit_t, robot_is_static)                 /* pin for notifying user that robot is in static */                             \
    FIELD(hal_bit_t, robot_is_boot)                   /* pin for notifying user that robot is booting */                               \
    FIELD(hal_bit_t, robot_is_emergency_stop)         /* pin for notifying user that robot is in emergency stop */                     \
    FIELD(hal_bit_t, robot_in_percentage_mode_level2) /* pin for notifying user that robot is in percentage mode level 2 */            \
    FIELD(hal_bit_t, robot_in_sys_protective_stop)    /* pin for notifying user that robot is in system protective stop */             \
    FIELD(hal_bit_t, robot_in_percentage_mode_level1) /*pin for notifying user that robot is in percentage mode level 1*/              \
    FIELD(hal_bit_t, program_run)                     /* pin for running program */                                                    \
    FIELD(hal_bit_t, program_auto_run)                     /* auto start program while enabled robot*/                                                    \
    FIELD(hal_bit_t, program_pause)                   /* pin for pausing program */                                                    \
    FIELD(hal_bit_t, collision_program_pause)                   /* pin for collision pausing program */                                                    \
    FIELD(hal_bit_t, program_resume)                  /* pin for resuming program */                                                   \
    FIELD(hal_bit_t, program_step)                    /* pin for running one line of the program */                                    \
    FIELD(hal_bit_t, program_stop)                    /* pin for stopping the program */                                               \
    FIELD(hal_bit_t, program_os_on)                   /* pin for setting optional stop on */                                           \
    FIELD(hal_bit_t, program_os_off)                  /* pin for setting optional stop off */                                          \
    FIELD(hal_bit_t, program_os_is_on)                /* status pin that optional stop is on */                                        \
    FIELD(hal_bit_t, program_bd_on)                   /* pin for setting block delete on */                                            \
    FIELD(hal_bit_t, program_bd_off)                  /* pin for setting block delete off */                                           \
    FIELD(hal_bit_t, program_bd_is_on)                /* status pin that block delete is on */                                         \
    FIELD(hal_bit_t, percentage_mode_level1)          /* robot percentage mode level 1 request */                                      \
    FIELD(hal_bit_t, percentage_mode_level2)          /* robot percentage mode level 2 request*/                                       \
    FIELD(hal_bit_t, reduce_mode)                     /*缩减模式*/                                                                 \
    FIELD(hal_s32_t, collision_lv0)                     /*碰撞灵敏度无碰撞*/                                                                 \
    FIELD(hal_s32_t, collision_lv1)                     /*碰撞灵敏度lv1*/                                                                 \
    FIELD(hal_s32_t, collision_lv2)                     /*碰撞灵敏度lv2*/                                                                 \
    FIELD(hal_s32_t, collision_lv3)                     /*碰撞灵敏度lv3*/                                                                 \
    FIELD(hal_s32_t, collision_lv4)                     /*碰撞灵敏度lv4*/                                                                 \
    FIELD(hal_s32_t, collision_lv5)                     /*碰撞灵敏度lv5*/                                                                 \
    FIELD(hal_s32_t, collision_lv7)                     /*碰撞灵敏度lv7*/                                                                 \
    FIELD(hal_bit_t, stop_mode)                       /*功能DI防护停止*/                                                         \
    FIELD(hal_bit_t, safetyguard_mode)                /*安全DI附加保护性停止*/                                                \
    FIELD(hal_bit_t, return_to_initial_position)      /**/                                                                             \
    FIELD(hal_bit_t, clear_error)                     /*  clear error   */                                                             \
    FIELD(hal_bit_t, mode_drag_exit)                  /* exit drag mode*/                                                              \
    FIELD(hal_bit_t, mode_drag_entr)                  /* entr drag mode*/                                                              \
    FIELD(hal_bit_t, sys_shutdown)                                                                                                     \
    FIELD(hal_s32_t, reset_safetyguard)                              /* reset safety guard*/                                           \
    FIELD(hal_bit_t, three_position_enabling)                        /*三位置使能退出手动运动或调试*/                    \
    ARRAY(hal_bit_t, joint_home, ZUCMOT_MAX_JOINTS + 1)              /* pin for homing one joint */                                    \
    ARRAY(hal_bit_t, joint_unhome, ZUCMOT_MAX_JOINTS + 1)            /* pin for unhoming one joint */                                  \
    ARRAY(hal_bit_t, joint_is_homed, ZUCMOT_MAX_JOINTS + 1)          /* status pin that the joint is homed */                          \
    ARRAY(hal_bit_t, joint_on_soft_min_limit, ZUCMOT_MAX_JOINTS + 1) /* status pin that the joint is on the software min limit */      \
    ARRAY(hal_bit_t, joint_on_soft_max_limit, ZUCMOT_MAX_JOINTS + 1) /* status pin that the joint is on the software max limit */      \
    ARRAY(hal_bit_t, joint_on_hard_min_limit, ZUCMOT_MAX_JOINTS + 1) /* status pin that the joint is on the hardware min limit */      \
    ARRAY(hal_bit_t, joint_on_hard_max_limit, ZUCMOT_MAX_JOINTS + 1) /* status pin that the joint is on the hardware max limit */      \
    ARRAY(hal_bit_t, joint_override_limits, ZUCMOT_MAX_JOINTS + 1)   /* status pin that the joint is on the hardware max limit */      \
    ARRAY(hal_bit_t, joint_has_fault, ZUCMOT_MAX_JOINTS + 1)         /* status pin that the joint has a fault */                       \
    FIELD(hal_u32_t, joint_selected)                                 /* status pin for the joint selected */                           \
    FIELD(hal_u32_t, axis_selected)                                  /* status pin for the axis selected */                            \
    ARRAY(hal_bit_t, joint_nr_select, ZUCMOT_MAX_JOINTS)             /* nr. of pins to select a joint */                               \
    ARRAY(hal_bit_t, axis_nr_select, ZUCMOT_MAX_AXIS)                /* nr. of pins to select a axis */                                \
    ARRAY(hal_bit_t, joint_is_selected, ZUCMOT_MAX_JOINTS)           /* nr. of status pins for joint selected */                       \
    ARRAY(hal_bit_t, axis_is_selected, ZUCMOT_MAX_AXIS)              /* nr. of status pins for axis selected */                        \
    ARRAY(hal_float_t, axis_pos_commanded, ZUCMOT_MAX_AXIS + 1)      /* status pin for commanded cartesian position */                 \
    ARRAY(hal_float_t, axis_pos_feedback, ZUCMOT_MAX_AXIS + 1)       /* status pin for actual cartesian position */                    \
    ARRAY(hal_float_t, axis_pos_relative, ZUCMOT_MAX_AXIS + 1)       /* status pin for relative cartesian position */                  \
    FIELD(hal_float_t, jjog_speed)                                   /* pin for setting the jog speed (halui internal) */              \
    ARRAY(hal_bit_t, jjog_minus, ZUCMOT_MAX_JOINTS + 1)              /* pin to jog in positive direction */                            \
    ARRAY(hal_bit_t, jjog_plus, ZUCMOT_MAX_JOINTS + 1)               /* pin to jog in negative direction */                            \
    ARRAY(hal_float_t, jjog_analog, ZUCMOT_MAX_JOINTS + 1)           /* pin for analog jogging (-1..0..1) */                           \
    ARRAY(hal_float_t, jjog_increment, ZUCMOT_MAX_JOINTS + 1)        /* Incremental jogging */                                         \
    ARRAY(hal_bit_t, jjog_increment_plus, ZUCMOT_MAX_JOINTS + 1)     /* Incremental jogging, positive direction */                     \
    ARRAY(hal_bit_t, jjog_increment_minus, ZUCMOT_MAX_JOINTS + 1)    /* Incremental jogging, negative direction */                     \
    FIELD(hal_float_t, ajog_speed)                                   /* pin for setting the jog speed (halui internal) */              \
    ARRAY(hal_bit_t, ajog_minus, ZUCMOT_MAX_AXIS + 1)                /* pin to jog in positive direction */                            \
    ARRAY(hal_bit_t, ajog_plus, ZUCMOT_MAX_AXIS + 1)                 /* pin to jog in negative direction */                            \
    ARRAY(hal_float_t, ajog_analog, ZUCMOT_MAX_AXIS + 1)             /* pin for analog jogging (-1..0..1) */                           \
    ARRAY(hal_float_t, ajog_increment, ZUCMOT_MAX_AXIS + 1)          /* Incremental jogging */                                         \
    ARRAY(hal_bit_t, ajog_increment_plus, ZUCMOT_MAX_AXIS + 1)       /* Incremental jogging, positive direction */                     \
    ARRAY(hal_bit_t, ajog_increment_minus, ZUCMOT_MAX_AXIS + 1)      /* Incremental jogging, negative direction */                     \
    FIELD(hal_float_t, jjog_deadband)                                /* pin for setting the jog analog deadband (where not to move) */ \
    FIELD(hal_float_t, ajog_deadband)                                /* pin for setting the jog analog deadband (where not to move) */ \
    FIELD(hal_s32_t, mv_counts)                                      /* pin for the Max Velocity counting */                           \
    FIELD(hal_bit_t, mv_count_enable)                                /* pin for the Max Velocity counting enable */                    \
    FIELD(hal_bit_t, mv_direct_value)                                /* pin for enabling direct value option instead of counts */      \
    FIELD(hal_float_t, mv_scale)                                     /* scale for the Max Velocity counting */                         \
    FIELD(hal_float_t, mv_value)                                     /* current Max Velocity value */                                  \
    FIELD(hal_bit_t, mv_increase)                                    /* pin for increasing the MV (+=scale) */                         \
    FIELD(hal_bit_t, mv_decrease)                                    /* pin for decreasing the MV (-=scale) */                         \
    FIELD(hal_s32_t, fo_counts)                                      /* pin for the Feed Override counting */                          \
    FIELD(hal_bit_t, fo_count_enable)                                /* pin for the Feed Override counting enable */                   \
    FIELD(hal_bit_t, fo_direct_value)                                /* pin for enabling direct value option instead of counts  */     \
    FIELD(hal_float_t, fo_scale)                                     /* scale for the Feed Override counting */                        \
    FIELD(hal_float_t, fo_value)                                     /* current Feed Override value */                                 \
    FIELD(hal_bit_t, fo_increase)                                    /* pin for increasing the FO (+=scale) */                         \
    FIELD(hal_bit_t, fo_decrease)                                    /* pin for decreasing the FO (-=scale) */                         \
    FIELD(hal_s32_t, ro_counts)                                      /* pin for the Feed Override counting */                          \
    FIELD(hal_bit_t, ro_count_enable)                                /* pin for the Feed Override counting enable */                   \
    FIELD(hal_bit_t, ro_direct_value)                                /* pin for enabling direct value option instead of counts  */     \
    FIELD(hal_float_t, ro_scale)                                     /* scale for the Feed Override counting */                        \
    FIELD(hal_float_t, ro_value)                                     /* current Feed Override value */                                 \
    FIELD(hal_bit_t, ro_increase)                                    /* pin ror increasing the FO (+=scale) */                         \
    FIELD(hal_bit_t, ro_decrease)                                    /* pin for decreasing the FO (-=scale) */                         \
    FIELD(hal_s32_t, so_counts)                                      /* pin for the Spindle Speed Override counting */                 \
    FIELD(hal_bit_t, so_count_enable)                                /* pin for the Spindle Speed Override counting enable */          \
    FIELD(hal_bit_t, so_direct_value)                                /* pin for enabling direct value option instead of counts */      \
    FIELD(hal_float_t, so_scale)                                     /* scale for the Spindle Speed Override counting */               \
    FIELD(hal_float_t, so_value)                                     /* current Spindle speed Override value */                        \
    FIELD(hal_bit_t, so_increase)                                    /* pin for increasing the SO (+=scale) */                         \
    FIELD(hal_bit_t, so_decrease)                                    /* pin for decreasing the SO (-=scale) */                         \
    FIELD(hal_bit_t, home_all)                                       /* pin for homing all joints in sequence */                       \
    FIELD(hal_bit_t, abort)                                          /* pin for aborting */                                            \
    ARRAY(hal_bit_t, mdi_commands, MDI_MAX)
// clang-format on

// clang-format on

struct PTR
{
    template <class T> struct field
    {
        typedef T* type;
    };
};

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wignored-attributes"
template <class T> struct NATIVE
{
};
template <> struct NATIVE<hal_bit_t>
{
    typedef bool type;
};
template <> struct NATIVE<hal_s32_t>
{
    typedef rtapi_s32 type;
};
template <> struct NATIVE<hal_u32_t>
{
    typedef rtapi_u32 type;
};
template <> struct NATIVE<hal_float_t>
{
    typedef double type;
};
struct VALUE
{
    template <class T> struct field
    {
        typedef typename NATIVE<T>::type type;
    };
};
#pragma GCC diagnostic pop

template <class T> struct halui_str_base
{
#define FIELD(t, f) typename T::template field<t>::type f;
#define ARRAY(t, f, n) typename T::template field<t>::type f[n];
    HAL_FIELDS
#undef FIELD
#undef ARRAY
};

typedef halui_str_base<PTR> halui_str;
typedef halui_str_base<VALUE> local_halui_str;

static halui_str* halui_data;
static local_halui_str old_halui_data;

static char* mdi_commands[MDI_MAX];
static int num_mdi_commands = 0;

static int comp_id, done; /* component ID, main while loop */

static int num_axes = 0;    //number of axes, taken from the ini [TRAJ] section
static int num_joints = 3;  //number of joints, taken from the ini [KINS] section

static double maxFeedOverride = 1;
static double maxMaxVelocity = 1;
static double minSpindleOverride = 0.0;
static double maxSpindleOverride = 1.0;
static ZUC_TASK_MODE_ENUM halui_old_mode = ZUC_TASK_MODE_MANUAL;
static int halui_sent_mdi = 0;

// the NML channels to the ZUC task
static RCS_CMD_CHANNEL* zucCommandBuffer = 0;
static RCS_STAT_CHANNEL* zucStatusBuffer = 0;
ZUC_STAT* zucStatus = 0;

// the NML channel for errors
static NML* zucErrorBuffer = 0;

// the serial number to use.
static int zucCommandSerialNumber = 0;

// how long to wait for Task to report that it has received our command
static double receiveTimeout = 5.0;

// how long to wait for Task to finish running our command
static double doneTimeout = 60.;

static void quit(int sig) { done = 1; }

static NML* zucErrorReportBuffer = 0;
#define DEFAULT_ZUC_UI_TIMEOUT 5.0
int zucErrorBufferOKtoWrite(int space, const char* caller)
{
    // check channel for validity
    if (zucErrorReportBuffer == NULL)
        return -1;
    if (!zucErrorReportBuffer->valid())
        return -1;

    double send_errorchan_timout = etime() + DEFAULT_ZUC_UI_TIMEOUT;

    while (etime() < send_errorchan_timout)
    {
        if (zucErrorReportBuffer->get_space_available() < space)
        {
            // esleep(0.01);
            // continue;
            return -1;
        }
        else
        {
            break;
        }
    }
    if (etime() >= send_errorchan_timout)
    {
        if (zuc_debug & ZUC_DEBUG_TASK_ISSUE)
        {
            rcs_print("timeout waiting for error channel to drain, caller=`%s' request=%d\n", caller, space);
        }
        return -1;
    }

    return 0;
}

int zucOperatorError(int id, const char* fmt, ...)
{
    ZUC_OPERATOR_ERROR error_msg;
    va_list ap;

    if (NULL == fmt)
    {
        return -1;
    }
    if (0 == *fmt)
    {
        return -1;
    }
    // prepend error code, leave off 0 ad-hoc code
    error_msg.errcode = id;
    error_msg.error[0] = 0;
    if (0 != id)
    {
        snprintf(error_msg.error, sizeof(error_msg.error), "[%d] ", id);
    }
    // append error string
    va_start(ap, fmt);
    vsnprintf(&error_msg.error[strlen(error_msg.error)], sizeof(error_msg.error) - strlen(error_msg.error), fmt, ap);
    va_end(ap);

    // force a NULL at the end for safety
    error_msg.error[LINELEN - 1] = 0;

    // write to zuc log file
    zucLogError(error_msg.errcode, error_msg.error);

    if (zucErrorBufferOKtoWrite(sizeof(error_msg) * 2, "zucOperatorError"))
        return -1;

    return zucErrorReportBuffer->write(error_msg);
}

int zucOperatorWarning(int id, const char* fmt, ...)
{
    ZUC_OPERATOR_ERROR error_msg;
    va_list ap;

    if (NULL == fmt)
    {
        return -1;
    }
    if (0 == *fmt)
    {
        return -1;
    }
    // prepend error code, leave off 0 ad-hoc code
    error_msg.errcode = id;
    error_msg.error[0] = 0;
    if (0 != id)
    {
        snprintf(error_msg.error, sizeof(error_msg.error), "[%d] ", id);
    }
    // append error string
    va_start(ap, fmt);
    vsnprintf(&error_msg.error[strlen(error_msg.error)], sizeof(error_msg.error) - strlen(error_msg.error), fmt, ap);
    va_end(ap);

    // force a NULL at the end for safety
    error_msg.error[LINELEN - 1] = 0;

    // write to zuc log file
    zucLogWarning(error_msg.errcode, error_msg.error);
    if (zucErrorBufferOKtoWrite(sizeof(error_msg) * 2, "zucOperatorError"))
        return -1;

    return zucErrorReportBuffer->write(error_msg);
}

int zucOperatorText(int id, const char* fmt, ...)
{
    ZUC_OPERATOR_TEXT text_msg;
    va_list ap;

    text_msg.id = id;
    // write args to NML message (ignore int text code)
    va_start(ap, fmt);
    vsnprintf(text_msg.text, sizeof(text_msg.text), fmt, ap);
    va_end(ap);
    // force a NULL at the end for safety
    text_msg.text[LINELEN - 1] = 0;

    // write to zuc log file
    zucLogInfo(text_msg.id, text_msg.text);

    if (zucErrorBufferOKtoWrite(sizeof(text_msg) * 2, "zucOperatorText"))
        return -1;

    return zucErrorReportBuffer->write(text_msg);
}

//# 适用于仅弹窗/飘窗，但是不记入errorlog
int zucOperatorDisplay(int id, const char* fmt, ...)
{
    ZUC_OPERATOR_DISPLAY display_msg;
    va_list ap;

    if (zucErrorBufferOKtoWrite(sizeof(display_msg) * 2, "zucOperatorDisplay"))
        return -1;

    // write args to NML message (ignore int display code)
    va_start(ap, fmt);
    vsnprintf(display_msg.display, sizeof(display_msg.display), fmt, ap);
    va_end(ap);

    display_msg.id = id;
    // force a NULL at the end for safety
    display_msg.display[LINELEN - 1] = 0;

    // write it
    return zucErrorReportBuffer->write(display_msg);
}

static int zucTaskNmlGet()
{
    int retval = 0;

    // try to connect to ZUC cmd
    if (zucCommandBuffer == 0)
    {
        zucCommandBuffer = new RCS_CMD_CHANNEL(zucFormat, "zucCommand", "xzuc", zuc_nmlfile);
        if (!zucCommandBuffer->valid())
        {
            delete zucCommandBuffer;
            zucCommandBuffer = 0;
            retval = -1;
        }
    }
    // try to connect to ZUC status
    if (zucStatusBuffer == 0)
    {
        zucStatusBuffer = new RCS_STAT_CHANNEL(zucFormat, "zucStatus", "xzuc", zuc_nmlfile);
        if (!zucStatusBuffer->valid())
        {
            delete zucStatusBuffer;
            zucStatusBuffer = 0;
            zucStatus = 0;
            retval = -1;
        }
        else
        {
            zucStatus = (ZUC_STAT*)zucStatusBuffer->get_address();
        }
    }

    return retval;
}

static int zucErrorNmlSet()
{
    int retval = 0;

    if (zucErrorReportBuffer == 0)
    {
        zucErrorReportBuffer = new NML(nmlErrorFormat, "zucError", "zuc", zuc_nmlfile);
        if (!zucErrorReportBuffer->valid())
        {
            delete zucErrorReportBuffer;
            zucErrorReportBuffer = 0;
            retval = -1;
        }
    }

    return retval;
}

static int zucErrorNmlGet()
{
    int retval = 0;

    if (zucErrorBuffer == 0)
    {
        zucErrorBuffer = new NML(nmlErrorFormat, "zucError", "xzuc", zuc_nmlfile);
        if (!zucErrorBuffer->valid())
        {
            delete zucErrorBuffer;
            zucErrorBuffer = 0;
            retval = -1;
        }
    }

    return retval;
}

static int tryNml()
{
    double end;
    int good;
#define RETRY_TIME 10.0     // seconds to wait for subsystems to come up
#define RETRY_INTERVAL 1.0  // seconds between wait tries for a subsystem

    if ((zuc_debug & ZUC_DEBUG_NML) == 0)
    {
        set_rcs_print_destination(RCS_PRINT_TO_NULL);  // inhibit diag
                                                       // messages
    }
    end = RETRY_TIME;
    good = 0;
    do {
        if (0 == zucTaskNmlGet())
        {
            good = 1;
            break;
        }
        esleep(RETRY_INTERVAL);
        end -= RETRY_INTERVAL;
    } while (end > 0.0);
    if ((zuc_debug & ZUC_DEBUG_NML) == 0)
    {
        set_rcs_print_destination(RCS_PRINT_TO_STDOUT);  // inhibit diag
                                                         // messages
    }
    if (!good)
    {
        return -1;
    }

    if ((zuc_debug & ZUC_DEBUG_NML) == 0)
    {
        set_rcs_print_destination(RCS_PRINT_TO_NULL);  // inhibit diag
                                                       // messages
    }
    end = RETRY_TIME;
    good = 0;
    do {
        if (0 == zucErrorNmlGet())
        {
            good = 1;
            break;
        }
        esleep(RETRY_INTERVAL);
        end -= RETRY_INTERVAL;
    } while (end > 0.0);

    end = RETRY_TIME;
    good = 0;
    do {
        if (0 == zucErrorNmlSet())
        {
            good = 1;
            break;
        }
        esleep(RETRY_INTERVAL);
        end -= RETRY_INTERVAL;
    } while (end > 0.0);

    if ((zuc_debug & ZUC_DEBUG_NML) == 0)
    {
        set_rcs_print_destination(RCS_PRINT_TO_STDOUT);  // inhibit diag
                                                         // messages
    }
    if (!good)
    {
        return -1;
    }

    return 0;

#undef RETRY_TIME
#undef RETRY_INTERVAL
}

static int updateStatus()
{
    static int errReported = 0;
    NMLTYPE type;

    if (0 == zucStatus || 0 == zucStatusBuffer)
    {
        rtapi_print("halui: %s: no status buffer\n", __func__);
        return -1;
    }

    if (!zucStatusBuffer->valid() && errReported == 0)
    {
        rtapi_print("halui: %s: status buffer is not valid\n", __func__);
        errReported = 1;
        return -1;
    }

    switch (type = zucStatusBuffer->peek())
    {
    case -1:
        // rtapi_print("halui: %s: error peeking status buffer\n", __func__);
        return -1;
        break;

    case 0:              // no new data
    case ZUC_STAT_TYPE:  // new data
        break;

    default:
        // rtapi_print("halui: %s: unknown error peeking status buffer\n", __func__);
        return -1;
        break;
    }

    return 0;
}

#define ZUC_COMMAND_DELAY 0.1  // how long to sleep between checks

static int zucCommandWaitDone()
{
    double end;
    for (end = 0.0; end < doneTimeout; end += ZUC_COMMAND_DELAY)
    {
        updateStatus();
        int serial_diff = zucStatus->echo_serial_number - zucCommandSerialNumber;

        if (serial_diff < 0)
        {
            continue;
        }

        if (serial_diff > 0)
        {
            return 0;
        }

        if (zucStatus->status == RCS_DONE)
        {
            return 0;
        }

        if (zucStatus->status == RCS_ERROR)
        {
            return -1;
        }

        esleep(ZUC_COMMAND_DELAY);
    }

    return -1;
}

static int zucCommandSend(RCS_CMD_MSG& cmd)
{
    // write command
    static int errReported = 0;
    if (zucCommandBuffer->write(&cmd) && errReported == 0)
    {
        rtapi_print("halui: %s: error writing to Task\n", __func__);
        errReported = 1;
        return -1;
    }
    zucCommandSerialNumber = cmd.serial_number;

    // wait for receive
    double end;
    for (end = 0.0; end < receiveTimeout; end += ZUC_COMMAND_DELAY)
    {
        updateStatus();
        int serial_diff = zucStatus->echo_serial_number - zucCommandSerialNumber;

        if (serial_diff >= 0)
        {
            return 0;
        }

        esleep(ZUC_COMMAND_DELAY);
    }

    return -1;
}

static void thisQuit()
{
    //don't forget the big HAL sin ;)
    hal_exit(comp_id);

    if (zucCommandBuffer)
    {
        delete zucCommandBuffer;
        zucCommandBuffer = 0;
    }
    if (zucStatusBuffer)
    {
        delete zucStatusBuffer;
        zucStatusBuffer = 0;
    }
    if (zucErrorBuffer)
    {
        delete zucErrorBuffer;
        zucErrorBuffer = 0;
    }

    if (zucErrorReportBuffer)
    {
        delete zucErrorReportBuffer;
        zucErrorReportBuffer = 0;
    }
    exit(0);
}

static enum { LINEAR_UNITS_CUSTOM = 1, LINEAR_UNITS_AUTO, LINEAR_UNITS_MM, LINEAR_UNITS_INCH, LINEAR_UNITS_CM } linearUnitConversion = LINEAR_UNITS_AUTO;

static enum {
    ANGULAR_UNITS_CUSTOM = 1,
    ANGULAR_UNITS_AUTO,
    ANGULAR_UNITS_DEG,
    ANGULAR_UNITS_RAD,
    ANGULAR_UNITS_GRAD
} angularUnitConversion = ANGULAR_UNITS_AUTO;

#define CLOSE(a, b, eps) ((a) - (b) < +(eps) && (a) - (b) > -(eps))
#define LINEAR_CLOSENESS 0.0001
#define ANGULAR_CLOSENESS 0.0001
//#define INCH_PER_MM (1.0 / 25.4)
#define CM_PER_MM 0.1
#define GRAD_PER_DEG (100.0 / 90.0)
#define RAD_PER_DEG TO_RAD  // from posemath.h

int halui_export_pin_IN_bit(hal_bit_t** pin, const char* name)
{
    int retval;
    retval = hal_pin_bit_new(name, HAL_IN, pin, comp_id);
    if (retval < 0)
    {
        rtapi_print_msg(RTAPI_MSG_ERR, "HALUI: ERROR: halui pin %s export failed with err=%i\n", name, retval);
        hal_exit(comp_id);
        return -1;
    }
    return 0;
}

int halui_export_pin_IN_s32(hal_s32_t** pin, const char* name)
{
    int retval;
    retval = hal_pin_s32_new(name, HAL_IN, pin, comp_id);
    if (retval < 0)
    {
        rtapi_print_msg(RTAPI_MSG_ERR, "HALUI: ERROR: halui pin %s export failed with err=%i\n", name, retval);
        hal_exit(comp_id);
        return -1;
    }
    return 0;
}

int halui_export_pin_IN_float(hal_float_t** pin, const char* name)
{
    int retval;
    retval = hal_pin_float_new(name, HAL_IN, pin, comp_id);
    if (retval < 0)
    {
        rtapi_print_msg(RTAPI_MSG_ERR, "HALUI: ERROR: halui pin %s export failed with err=%i\n", name, retval);
        hal_exit(comp_id);
        return -1;
    }
    return 0;
}

int halui_export_pin_OUT_bit(hal_bit_t** pin, const char* name)
{
    int retval;
    retval = hal_pin_bit_new(name, HAL_OUT, pin, comp_id);
    if (retval < 0)
    {
        rtapi_print_msg(RTAPI_MSG_ERR, "HALUI: ERROR: halui pin %s export failed with err=%i\n", name, retval);
        hal_exit(comp_id);
        return -1;
    }
    return 0;
}

/********************************************************************
*
* Description: halui_hal_init(void)
*
* Side Effects: Exports HAL pins.
*
* Called By: main
********************************************************************/
int halui_hal_init(void)
{
    int retval;
    int joint;
    int axis_num;

    /* STEP 1: initialise the hal component */
    comp_id = hal_init("halui");
    if (comp_id < 0)
    {
        rtapi_print_msg(RTAPI_MSG_ERR, "HALUI: ERROR: hal_init() failed\n");
        return -1;
    }

    /* STEP 2: allocate shared memory for halui data */
    halui_data = (halui_str*)hal_malloc(sizeof(halui_str));
    if (halui_data == 0)
    {
        rtapi_print_msg(RTAPI_MSG_ERR, "HALUI: ERROR: hal_malloc() failed\n");
        hal_exit(comp_id);
        return -1;
    }

    /* STEP 3a: export the out-pin(s) */

    retval = halui_export_pin_OUT_bit(&(halui_data->machine_is_on), "halui.machine.is-on");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->estop_is_activated), "halui.estop.is-activated");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->mode_is_manual), "halui.mode.is-manual");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->mode_is_auto), "halui.mode.is-auto");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->mode_is_mdi), "halui.mode.is-mdi");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->mode_is_drag), "halui.mode.is-drag");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->mode_is_teleop), "halui.mode.is-teleop");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->mode_is_teleop_tool), "halui.mode.is-teleop_tool");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->mode_is_joint), "halui.mode.is-joint");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->mist_is_on), "halui.mist.is-on");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->flood_is_on), "halui.flood.is-on");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->lube_is_on), "halui.lube.is-on");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->program_is_idle), "halui.program.is-idle");  //
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->program_is_running), "halui.program.is-running");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->stepping_is_running), "halui.stepping.is-running");
    if (retval < 0)
        return retval;

    retval = halui_export_pin_OUT_bit(&(halui_data->program_is_paused), "halui.program.is-paused");
    if (retval < 0)
        return retval;

    retval = halui_export_pin_OUT_bit(&(halui_data->robot_is_poweron), "halui.robot.is-poweron");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->robot_is_enabled), "halui.robot.is-enabled");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->robot_is_motion), "halui.robot.is-motion");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->robot_is_static), "halui.robot.is-static");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->robot_is_boot), "halui.robot.is-boot");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->robot_is_emergency_stop), "halui.robot.is-emergency-stop");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->robot_in_percentage_mode_level2), "halui.robot.in-percentage-mode-level2");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->robot_in_sys_protective_stop), "halui.robot.in-sys-protective-stop");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit((&halui_data->robot_in_percentage_mode_level1), "halui.robot.in-percentage-mode-level1");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->program_os_is_on), "halui.program.optional-stop.is-on");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_OUT_bit(&(halui_data->program_bd_is_on), "halui.program.block-delete.is-on");
    if (retval < 0)
        return retval;

    for (joint = 0; joint < num_joints; joint++)
    {
        retval = hal_pin_bit_newf(HAL_OUT, &(halui_data->joint_is_homed[joint]), comp_id, "halui.joint.%d.is-homed", joint);
        if (retval < 0)
            return retval;
        retval = hal_pin_bit_newf(HAL_OUT, &(halui_data->joint_is_selected[joint]), comp_id, "halui.joint.%d.is-selected", joint);
        if (retval < 0)
            return retval;
        retval = hal_pin_bit_newf(HAL_OUT, &(halui_data->joint_on_soft_min_limit[joint]), comp_id, "halui.joint.%d.on-soft-min-limit", joint);
        if (retval < 0)
            return retval;
        retval = hal_pin_bit_newf(HAL_OUT, &(halui_data->joint_on_soft_max_limit[joint]), comp_id, "halui.joint.%d.on-soft-max-limit", joint);
        if (retval < 0)
            return retval;
        retval = hal_pin_bit_newf(HAL_OUT, &(halui_data->joint_on_hard_min_limit[joint]), comp_id, "halui.joint.%d.on-hard-min-limit", joint);
        if (retval < 0)
            return retval;
        retval = hal_pin_bit_newf(HAL_OUT, &(halui_data->joint_on_hard_max_limit[joint]), comp_id, "halui.joint.%d.on-hard-max-limit", joint);
        if (retval < 0)
            return retval;
        retval = hal_pin_bit_newf(HAL_OUT, &(halui_data->joint_override_limits[joint]), comp_id, "halui.joint.%d.override-limits", joint);
        if (retval < 0)
            return retval;
        retval = hal_pin_bit_newf(HAL_OUT, &(halui_data->joint_has_fault[joint]), comp_id, "halui.joint.%d.has-fault", joint);
        if (retval < 0)
            return retval;
    }

    retval = hal_pin_bit_newf(HAL_OUT, &(halui_data->joint_on_soft_min_limit[num_joints]), comp_id, "halui.joint.selected.on-soft-min-limit");
    if (retval < 0)
        return retval;
    retval = hal_pin_bit_newf(HAL_OUT, &(halui_data->joint_on_soft_max_limit[num_joints]), comp_id, "halui.joint.selected.on-soft-max-limit");
    if (retval < 0)
        return retval;
    retval = hal_pin_bit_newf(HAL_OUT, &(halui_data->joint_on_hard_min_limit[num_joints]), comp_id, "halui.joint.selected.on-hard-min-limit");
    if (retval < 0)
        return retval;
    retval = hal_pin_bit_newf(HAL_OUT, &(halui_data->joint_on_hard_max_limit[num_joints]), comp_id, "halui.joint.selected.on-hard-max-limit");
    if (retval < 0)
        return retval;
    retval = hal_pin_bit_newf(HAL_OUT, &(halui_data->joint_override_limits[num_joints]), comp_id, "halui.joint.selected.override-limits");
    if (retval < 0)
        return retval;
    retval = hal_pin_bit_newf(HAL_OUT, &(halui_data->joint_has_fault[num_joints]), comp_id, "halui.joint.selected.has-fault");
    if (retval < 0)
        return retval;
    retval = hal_pin_bit_newf(HAL_OUT, &(halui_data->joint_is_homed[num_joints]), comp_id, "halui.joint.selected.is-homed");
    if (retval < 0)
        return retval;

    for (axis_num = 0; axis_num < ZUCMOT_MAX_AXIS; axis_num++)
    {
        if (!(axis_mask & (1 << axis_num)))
        {
            continue;
        }
        char c = "xyzabcuvw"[axis_num];

        retval = hal_pin_bit_newf(HAL_OUT, &(halui_data->axis_is_selected[axis_num]), comp_id, "halui.axis.%c.is-selected", c);
        if (retval < 0)
            return retval;
        retval = hal_pin_float_newf(HAL_OUT, &(halui_data->axis_pos_commanded[axis_num]), comp_id, "halui.axis.%c.pos-commanded", c);
        if (retval < 0)
            return retval;
        retval = hal_pin_float_newf(HAL_OUT, &(halui_data->axis_pos_feedback[axis_num]), comp_id, "halui.axis.%c.pos-feedback", c);
        if (retval < 0)
            return retval;
        retval = hal_pin_float_newf(HAL_OUT, &(halui_data->axis_pos_relative[axis_num]), comp_id, "halui.axis.%c.pos-relative", c);
        if (retval < 0)
            return retval;
    }

    // at startup, indicate [0] item is selected:
    *halui_data->joint_is_selected[0] = 1;
    *halui_data->axis_is_selected[0] = 1;

    retval = hal_pin_float_newf(HAL_OUT, &(halui_data->mv_value), comp_id, "halui.max-velocity.value");
    if (retval < 0)
        return retval;
    retval = hal_pin_float_newf(HAL_OUT, &(halui_data->fo_value), comp_id, "halui.feed-override.value");
    if (retval < 0)
        return retval;
    retval = hal_pin_float_newf(HAL_OUT, &(halui_data->ro_value), comp_id, "halui.rapid-override.value");
    if (retval < 0)
        return retval;
    retval = hal_pin_u32_newf(HAL_OUT, &(halui_data->joint_selected), comp_id, "halui.joint.selected");
    if (retval < 0)
        return retval;
    retval = hal_pin_u32_newf(HAL_OUT, &(halui_data->axis_selected), comp_id, "halui.axis.selected");
    if (retval < 0)
        return retval;
    retval = hal_pin_float_newf(HAL_OUT, &(halui_data->so_value), comp_id, "halui.spindle-override.value");
    if (retval < 0)
        return retval;

    /* STEP 3b: export the in-pin(s) */
    retval = halui_export_pin_IN_bit(&(halui_data->sys_shutdown), "halui.sys-shutdown");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->machine_on), "halui.machine.on");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->bp_enable), "halui.bp.enable");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->machine_off), "halui.machine.off");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->estop_activate), "halui.estop.activate");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->estop_reset), "halui.estop.reset");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->mode_manual), "halui.mode.manual");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->mode_auto), "halui.mode.auto");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->mode_mdi), "halui.mode.mdi");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->mode_drag), "halui.mode.drag");  //
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->mode_teleop), "halui.mode.teleop");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->mode_teleop_tool), "halui.mode.teleop_tool");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->mode_joint), "halui.mode.joint");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->mist_on), "halui.mist.on");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->mist_off), "halui.mist.off");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->flood_on), "halui.flood.on");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->flood_off), "halui.flood.off");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->lube_on), "halui.lube.on");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->lube_off), "halui.lube.off");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->program_run), "halui.program.run");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->program_auto_run), "halui.program.auto.run");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->program_pause), "halui.program.pause");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->collision_program_pause), "halui.collision.program.pause");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->program_resume), "halui.program.resume");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->program_step), "halui.program.step");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->program_stop), "halui.program.stop");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->program_os_on), "halui.program.optional-stop.on");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->program_os_off), "halui.program.optional-stop.off");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->program_bd_on), "halui.program.block-delete.on");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->program_bd_off), "halui.program.block-delete.off");
    if (retval < 0)
        return retval;

    retval = halui_export_pin_IN_bit(&(halui_data->return_to_initial_position), "halui.return_to_initial_position");
    if (retval < 0)
        return retval;

    retval = halui_export_pin_IN_bit(&(halui_data->percentage_mode_level1), "halui.percentage-level1.mode");
    if (retval < 0)
        return retval;

    retval = halui_export_pin_IN_bit(&(halui_data->percentage_mode_level2), "halui.percentage-level2.mode");
    if (retval < 0)
        return retval;

    retval = halui_export_pin_IN_bit(&(halui_data->reduce_mode), "halui.reduce.mode");
    if (retval < 0)
        return retval;

    retval = halui_export_pin_IN_s32(&(halui_data->collision_lv0), "halui.collision.level0");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_s32(&(halui_data->collision_lv1), "halui.collision.level1");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_s32(&(halui_data->collision_lv2), "halui.collision.level2");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_s32(&(halui_data->collision_lv3), "halui.collision.level3");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_s32(&(halui_data->collision_lv4), "halui.collision.level4");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_s32(&(halui_data->collision_lv5), "halui.collision.level5");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_s32(&(halui_data->collision_lv7), "halui.collision.level7");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->stop_mode), "halui.stop.mode");
    if (retval < 0)
        return retval;

    retval = halui_export_pin_IN_bit(&(halui_data->safetyguard_mode), "halui.safetyguard.mode");
    if (retval < 0)
        return retval;

    retval = halui_export_pin_IN_bit(&(halui_data->clear_error), "halui.clear_error");  //
    if (retval < 0)
        return retval;

    retval = halui_export_pin_IN_bit(&(halui_data->mode_drag_exit), "halui.mode-drag-exit");
    if (retval < 0)
        return retval;

    retval = halui_export_pin_IN_bit(&(halui_data->mode_drag_entr), "halui.mode-drag-entr");
    if (retval < 0)
        return retval;

    retval = halui_export_pin_IN_s32(&(halui_data->reset_safetyguard), "halui.reset-safetyguard");
    if (retval < 0)
        return retval;

    retval = halui_export_pin_IN_bit(&(halui_data->three_position_enabling), "halui.three-position-enabling");
    if (retval < 0)
        return retval;

    retval = halui_export_pin_IN_s32(&(halui_data->mv_counts), "halui.max-velocity.counts");
    if (retval < 0)
        return retval;
    *halui_data->mv_counts = 0;
    retval = halui_export_pin_IN_bit(&(halui_data->mv_count_enable), "halui.max-velocity.count-enable");
    if (retval < 0)
        return retval;
    *halui_data->mv_count_enable = 1;
    retval = halui_export_pin_IN_bit(&(halui_data->mv_direct_value), "halui.max-velocity.direct-value");
    if (retval < 0)
        return retval;
    *halui_data->mv_direct_value = 0;
    retval = halui_export_pin_IN_float(&(halui_data->mv_scale), "halui.max-velocity.scale");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->mv_increase), "halui.max-velocity.increase");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->mv_decrease), "halui.max-velocity.decrease");
    if (retval < 0)
        return retval;

    retval = halui_export_pin_IN_s32(&(halui_data->fo_counts), "halui.feed-override.counts");
    if (retval < 0)
        return retval;
    *halui_data->fo_counts = 0;
    retval = halui_export_pin_IN_bit(&(halui_data->fo_count_enable), "halui.feed-override.count-enable");
    if (retval < 0)
        return retval;
    *halui_data->fo_count_enable = 1;
    retval = halui_export_pin_IN_bit(&(halui_data->fo_direct_value), "halui.feed-override.direct-value");
    if (retval < 0)
        return retval;
    *halui_data->fo_direct_value = 0;
    retval = halui_export_pin_IN_float(&(halui_data->fo_scale), "halui.feed-override.scale");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->fo_increase), "halui.feed-override.increase");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->fo_decrease), "halui.feed-override.decrease");
    if (retval < 0)
        return retval;

    retval = halui_export_pin_IN_s32(&(halui_data->ro_counts), "halui.rapid-override.counts");
    if (retval < 0)
        return retval;
    *halui_data->ro_counts = 0;
    retval = halui_export_pin_IN_bit(&(halui_data->ro_count_enable), "halui.rapid-override.count-enable");
    if (retval < 0)
        return retval;
    *halui_data->ro_count_enable = 1;
    retval = halui_export_pin_IN_bit(&(halui_data->ro_direct_value), "halui.rapid-override.direct-value");
    if (retval < 0)
        return retval;
    *halui_data->ro_direct_value = 0;
    retval = halui_export_pin_IN_float(&(halui_data->ro_scale), "halui.rapid-override.scale");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->ro_increase), "halui.rapid-override.increase");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->ro_decrease), "halui.rapid-override.decrease");
    if (retval < 0)
        return retval;

    retval = halui_export_pin_IN_s32(&(halui_data->so_counts), "halui.spindle-override.counts");
    if (retval < 0)
        return retval;
    *halui_data->so_counts = 0;
    retval = halui_export_pin_IN_bit(&(halui_data->so_count_enable), "halui.spindle-override.count-enable");
    if (retval < 0)
        return retval;
    *halui_data->so_count_enable = 1;
    retval = halui_export_pin_IN_bit(&(halui_data->so_direct_value), "halui.spindle-override.direct-value");
    if (retval < 0)
        return retval;
    *halui_data->so_direct_value = 0;
    retval = halui_export_pin_IN_float(&(halui_data->so_scale), "halui.spindle-override.scale");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->so_increase), "halui.spindle-override.increase");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_bit(&(halui_data->so_decrease), "halui.spindle-override.decrease");
    if (retval < 0)
        return retval;

    retval = halui_export_pin_IN_bit(&(halui_data->home_all), "halui.home-all");
    if (retval < 0)
        return retval;

    retval = halui_export_pin_IN_bit(&(halui_data->abort), "halui.abort");
    if (retval < 0)
        return retval;

    for (joint = 0; joint < num_joints; joint++)
    {
        retval = hal_pin_bit_newf(HAL_IN, &(halui_data->joint_home[joint]), comp_id, "halui.joint.%d.home", joint);
        if (retval < 0)
            return retval;
        retval = hal_pin_bit_newf(HAL_IN, &(halui_data->joint_unhome[joint]), comp_id, "halui.joint.%d.unhome", joint);
        if (retval < 0)
            return retval;
        retval = hal_pin_bit_newf(HAL_IN, &(halui_data->joint_nr_select[joint]), comp_id, "halui.joint.%d.select", joint);
        if (retval < 0)
            return retval;
        retval = hal_pin_bit_newf(HAL_IN, &(halui_data->jjog_plus[joint]), comp_id, "halui.joint.%d.plus", joint);
        if (retval < 0)
            return retval;
        retval = hal_pin_bit_newf(HAL_IN, &(halui_data->jjog_minus[joint]), comp_id, "halui.joint.%d.minus", joint);
        if (retval < 0)
            return retval;
        retval = hal_pin_float_newf(HAL_IN, &(halui_data->jjog_analog[joint]), comp_id, "halui.joint.%d.analog", joint);
        if (retval < 0)
            return retval;
        retval = hal_pin_float_newf(HAL_IN, &(halui_data->jjog_increment[joint]), comp_id, "halui.joint.%d.increment", joint);
        if (retval < 0)
            return retval;
        retval = hal_pin_bit_newf(HAL_IN, &(halui_data->jjog_increment_plus[joint]), comp_id, "halui.joint.%d.increment-plus", joint);
        if (retval < 0)
            return retval;
        retval = hal_pin_bit_newf(HAL_IN, &(halui_data->jjog_increment_minus[joint]), comp_id, "halui.joint.%d.increment-minus", joint);
        if (retval < 0)
            return retval;
    }

    for (axis_num = 0; axis_num < ZUCMOT_MAX_AXIS; axis_num++)
    {
        char c = "xyzabcuvw"[axis_num];
        retval = hal_pin_bit_newf(HAL_IN, &(halui_data->axis_nr_select[axis_num]), comp_id, "halui.axis.%c.select", c);
        if (retval < 0)
            return retval;
        retval = hal_pin_bit_newf(HAL_IN, &(halui_data->ajog_plus[axis_num]), comp_id, "halui.axis.%c.plus", c);
        if (retval < 0)
            return retval;
        retval = hal_pin_bit_newf(HAL_IN, &(halui_data->ajog_minus[axis_num]), comp_id, "halui.axis.%c.minus", c);
        if (retval < 0)
            return retval;
        retval = hal_pin_float_newf(HAL_IN, &(halui_data->ajog_analog[axis_num]), comp_id, "halui.axis.%c.analog", c);
        if (retval < 0)
            return retval;
        retval = hal_pin_float_newf(HAL_IN, &(halui_data->ajog_increment[axis_num]), comp_id, "halui.axis.%c.increment", c);
        if (retval < 0)
            return retval;
        retval = hal_pin_bit_newf(HAL_IN, &(halui_data->ajog_increment_plus[axis_num]), comp_id, "halui.axis.%c.increment-plus", c);
        if (retval < 0)
            return retval;
        retval = hal_pin_bit_newf(HAL_IN, &(halui_data->ajog_increment_minus[axis_num]), comp_id, "halui.axis.%c.increment-minus", c);
        if (retval < 0)
            return retval;
    }

    retval = hal_pin_bit_newf(HAL_IN, &(halui_data->joint_home[num_joints]), comp_id, "halui.joint.selected.home");
    if (retval < 0)
        return retval;
    retval = hal_pin_bit_newf(HAL_IN, &(halui_data->joint_unhome[num_joints]), comp_id, "halui.joint.selected.unhome");
    if (retval < 0)
        return retval;
    retval = hal_pin_bit_newf(HAL_IN, &(halui_data->jjog_plus[num_joints]), comp_id, "halui.joint.selected.plus");
    if (retval < 0)
        return retval;
    retval = hal_pin_bit_newf(HAL_IN, &(halui_data->jjog_minus[num_joints]), comp_id, "halui.joint.selected.minus");
    if (retval < 0)
        return retval;
    retval = hal_pin_float_newf(HAL_IN, &(halui_data->jjog_increment[num_joints]), comp_id, "halui.joint.selected.increment");
    if (retval < 0)
        return retval;
    retval = hal_pin_bit_newf(HAL_IN, &(halui_data->jjog_increment_plus[num_joints]), comp_id, "halui.joint.selected.increment-plus");
    if (retval < 0)
        return retval;
    retval = hal_pin_bit_newf(HAL_IN, &(halui_data->jjog_increment_minus[num_joints]), comp_id, "halui.joint.selected.increment-minus");
    if (retval < 0)
        return retval;
    retval = hal_pin_bit_newf(HAL_IN, &(halui_data->ajog_plus[ZUCMOT_MAX_AXIS]), comp_id, "halui.axis.selected.plus");
    if (retval < 0)
        return retval;
    retval = hal_pin_bit_newf(HAL_IN, &(halui_data->ajog_minus[ZUCMOT_MAX_AXIS]), comp_id, "halui.axis.selected.minus");
    if (retval < 0)
        return retval;
    retval = hal_pin_float_newf(HAL_IN, &(halui_data->ajog_increment[ZUCMOT_MAX_AXIS]), comp_id, "halui.axis.selected.increment");
    if (retval < 0)
        return retval;
    retval = hal_pin_bit_newf(HAL_IN, &(halui_data->ajog_increment_plus[ZUCMOT_MAX_AXIS]), comp_id, "halui.axis.selected.increment-plus");
    if (retval < 0)
        return retval;
    retval = hal_pin_bit_newf(HAL_IN, &(halui_data->ajog_increment_minus[ZUCMOT_MAX_AXIS]), comp_id, "halui.axis.selected.increment-minus");
    if (retval < 0)
        return retval;

    retval = halui_export_pin_IN_float(&(halui_data->jjog_speed), "halui.joint.jog-speed");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_float(&(halui_data->jjog_deadband), "halui.joint.jog-deadband");
    if (retval < 0)
        return retval;

    retval = halui_export_pin_IN_float(&(halui_data->ajog_speed), "halui.axis.jog-speed");
    if (retval < 0)
        return retval;
    retval = halui_export_pin_IN_float(&(halui_data->ajog_deadband), "halui.axis.jog-deadband");
    if (retval < 0)
        return retval;

    for (int n = 0; n < num_mdi_commands; n++)
    {
        retval = hal_pin_bit_newf(HAL_IN, &(halui_data->mdi_commands[n]), comp_id, "halui.mdi-command-%02d", n);
        if (retval < 0)
            return retval;
    }

    hal_ready(comp_id);
    return 0;
}

static int sendMachineOn()
{
    ZUC_TASK_SET_STATE state_msg;

    state_msg.state = ZUC_TASK_STATE_ON;
    return zucCommandSend(state_msg);
}

static int sendMachineOff()
{
    ZUC_TASK_SET_STATE state_msg;

    state_msg.state = ZUC_TASK_STATE_OFF;
    return zucCommandSend(state_msg);
}

static int sendEstop()
{
    ZUC_TASK_SET_STATE state_msg;

    state_msg.state = ZUC_TASK_STATE_POWERED_OFF;
    return zucCommandSend(state_msg);
}

static int sendEstopReset()
{
    ZUC_TASK_SET_STATE state_msg;

    state_msg.state = ZUC_TASK_STATE_POWERED_ON;
    return zucCommandSend(state_msg);
}

static int sendManual()
{
    ZUC_TASK_SET_MODE mode_msg;
    if (zucStatus->task.mode == ZUC_TASK_MODE_MANUAL)
    {
        return 0;
    }

    mode_msg.mode = ZUC_TASK_MODE_MANUAL;
    return zucCommandSend(mode_msg);
}

static int sendAuto()
{
    ZUC_TASK_SET_MODE mode_msg;

    if (zucStatus->task.mode == ZUC_TASK_MODE_AUTO)
    {
        return 0;
    }

    mode_msg.mode = ZUC_TASK_MODE_AUTO;
    return zucCommandSend(mode_msg);
}

static int sendMdi()
{
    ZUC_TASK_SET_MODE mode_msg;

    if (zucStatus->task.mode == ZUC_TASK_MODE_MDI)
    {
        return 0;
    }

    mode_msg.mode = ZUC_TASK_MODE_MDI;
    return zucCommandSend(mode_msg);
}

static int sendDrag()  //
{
    ZUC_TASK_SET_MODE mode_msg;

    if (zucStatus->task.mode == ZUC_TASK_MODE_DRAG)
    {
        return 0;
    }

    mode_msg.mode = ZUC_TASK_MODE_DRAG;
    return zucCommandSend(mode_msg);
}

static int sendMdiCommand(int n)
{
    ZUC_TASK_PLAN_EXECUTE zuc_task_plan_execute_msg;

    if (updateStatus())
    {
        return -1;
    }

    if (!halui_sent_mdi)
    {
        // There is currently no MDI command from halui executing, we're
        // currently starting the first one.  Record what the Task mode is,
        // so we can restore it when all the MDI commands finish.
        halui_old_mode = zucStatus->task.mode;
    }

    // switch to MDI mode if needed
    if (zucStatus->task.mode != ZUC_TASK_MODE_MDI)
    {
        if (sendMdi() != 0)
        {
            rtapi_print("halui: %s: failed to Set Mode MDI\n", __func__);
            return -1;
        }
        if (updateStatus() != 0)
        {
            rtapi_print("halui: %s: failed to update status\n", __func__);
            return -1;
        }
        if (zucStatus->task.mode != ZUC_TASK_MODE_MDI)
        {
            rtapi_print("halui: %s: switched mode, but got %d instead of mdi\n", __func__, zucStatus->task.mode);
            return -1;
        }
    }
    strcpy(zuc_task_plan_execute_msg.command, mdi_commands[n]);
    if (zucCommandSend(zuc_task_plan_execute_msg))
    {
        rtapi_print("halui: %s: failed to send mdi command %d\n", __func__, n);
        return -1;
    }
    halui_sent_mdi = 1;
    return 0;
}

static int sendTeleop()
{
    ZUC_TRAJ_SET_TELEOP_ENABLE zuc_set_teleop_enable_msg;

    zuc_set_teleop_enable_msg.enable = 1;
    if (zucCommandSend(zuc_set_teleop_enable_msg))
    {
        return -1;
    }
    return zucCommandWaitDone();
}

static int sendTeleop_tool()
{
    ZUC_TRAJ_SET_TELEOP_TOOL_ENABLE zuc_set_teleop_tool_enable_msg;

    zuc_set_teleop_tool_enable_msg.enable = 1;
    if (zucCommandSend(zuc_set_teleop_tool_enable_msg))
    {
        return -1;
    }
    return zucCommandWaitDone();
}

static int sendJoint()
{
    ZUC_TRAJ_SET_TELEOP_ENABLE zuc_set_teleop_enable_msg;

    zuc_set_teleop_enable_msg.enable = 0;
    if (zucCommandSend(zuc_set_teleop_enable_msg))
    {
        return -1;
    }

    ZUC_TRAJ_SET_TELEOP_TOOL_ENABLE zuc_set_teleop_tool_enable_msg;
    zuc_set_teleop_tool_enable_msg.enable = 0;
    if (zucCommandSend(zuc_set_teleop_tool_enable_msg))
    {
        return -1;
    }

    return zucCommandWaitDone();
}

// static int sendMistOn()
// {
//     return 0;
// }

// static int sendMistOff()
// {
//     return 0;
// }

// static int sendFloodOn()
// {
//     return 0;
// }

// static int sendFloodOff()
// {
//     return 0;
// }

// static int sendLubeOn()
// {
//     return 0;
// }

// static int sendLubeOff()
// {
//     return 0;
// }

// programStartLine is the saved valued of the line that
// sendProgramRun(int line) sent
static int programStartLine = 0;

static int sendHome(int joint);

static int sendProgramOpen(char* filename)
{
    ZUC_TASK_PLAN_CLOSE close_program;
    zucCommandSend(close_program);

    ZUC_TASK_PLAN_OPEN open_program;
    memset(open_program.file, '\0', LINELEN);
    if (filename == nullptr)  //未传入参数
    {
        return -1;
    }
    else
    {
        strncpy(open_program.file, filename, LINELEN);
    }

    return zucCommandSend(open_program);
}

static int sendProgramRun(int line)
{
    printf(" [sendProgramRun] : \n");
    ZUC_TASK_PLAN_RUN zuc_task_plan_run_msg;

    ZUC_TASK_PLAN_SYNCH synch;

    updateStatus();

    if ((0 == zucStatus->task.file_auto_mode[0] && zucStatus->operation_mode == 1) ||
        (zucStatus->task.file_manu_mode[0] == 0 && zucStatus->operation_mode == 0))
    {
        printf(" 0 == zucStatus->task.file[0] \n");
        return -1;  // no program open
    }

    sendJoint();

    if (!zucStatus->motion.joint[0].homed)
        sendHome(-1);

    zucCommandSend(synch);

    // save the start line, to compare against active line later
    programStartLine = line;

    zuc_task_plan_run_msg.line = line;
    sendAuto();
    return zucCommandSend(zuc_task_plan_run_msg);
}

static int sendProgramAutoRun(int line)
{
    /**
     * 1、关闭当前程序
     * 2、打开默认程序
     * 3、ZUC_TASK_PLAN_SYNCH
     * 4、运行程序
    */
    printf("Enabled  --->  [sendProgramAutoRun] : \n");
    ZUC_TASK_PLAN_CLOSE close_current_opened_program_file;
    ZUC_TASK_PLAN_OPEN open_default_programp_file;
    ZUC_TASK_PLAN_RUN zuc_task_plan_run_msg;
    ZUC_TASK_PLAN_SYNCH synch;

    zucCommandSend(close_current_opened_program_file);
    printf("Enabled  --->  [ZUC_TASK_PLAN_CLOSE] : \n");

    updateStatus();
    strncpy(open_default_programp_file.file, zucStatus->task.autoWork.filename, LINELEN);
    printf("Enabled  --->  [ZUC_TASK_PLAN_OPEN] :  %s \n", open_default_programp_file.file);
    zucCommandSend(open_default_programp_file);

    if (0 == zucStatus->task.file[0])
    {
        printf(" 0 == zucStatus->task.file[0] \n");
        return -1;  // no program open
    }

    sendJoint();

    if (!zucStatus->motion.joint[0].homed)
        sendHome(-1);

    zucCommandSend(synch);

    // save the start line, to compare against active line later
    programStartLine = line;

    zuc_task_plan_run_msg.line = line;
    sendAuto();
    return zucCommandSend(zuc_task_plan_run_msg);
}

static int sendProgramPause()
{
    ZUC_TASK_PLAN_PAUSE zuc_task_plan_pause_msg;

    return zucCommandSend(zuc_task_plan_pause_msg);
}

static int sendSetOptionalStop(bool state)
{
    ZUC_TASK_PLAN_SET_OPTIONAL_STOP zuc_task_plan_set_optional_stop_msg;

    zuc_task_plan_set_optional_stop_msg.state = state;
    return zucCommandSend(zuc_task_plan_set_optional_stop_msg);
}

static int sendSetBlockDelete(bool state)
{
    ZUC_TASK_PLAN_SET_BLOCK_DELETE zuc_task_plan_set_block_delete_msg;

    zuc_task_plan_set_block_delete_msg.state = state;
    return zucCommandSend(zuc_task_plan_set_block_delete_msg);
}

static int sendProgramResume()
{
    ZUC_TASK_PLAN_RESUME zuc_task_plan_resume_msg;

    return zucCommandSend(zuc_task_plan_resume_msg);
}

static int sendProgramStep()
{
    ZUC_TASK_PLAN_STEP zuc_task_plan_step_msg;

    return zucCommandSend(zuc_task_plan_step_msg);
}

static int sendHome(int joint)
{
    return 0;
    // ZUC_JOINT_HOME zuc_joint_home_msg;

    // zuc_joint_home_msg.joint = joint;
    // return zucCommandSend(zuc_joint_home_msg);
}

static int sendUnhome(int joint)
{
    return 0;
    // ZUC_JOINT_UNHOME zuc_joint_unhome_msg;

    // zuc_joint_unhome_msg.joint = joint;
    // return zucCommandSend(zuc_joint_unhome_msg);
}

static int sendAbort()
{
    ZUC_TASK_ABORT task_abort_msg;

    return zucCommandSend(task_abort_msg);
}

static void sendJogStop(int ja, int jjogmode)
{
    ZUC_JOG_STOP zuc_jog_stop_msg;

    if (((jjogmode == JOGJOINT) && (zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_TELEOP || zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_TELEOP_TOOL)) ||
        ((jjogmode == JOGTELEOP) && (zucStatus->motion.traj.mode != ZUC_TRAJ_MODE_TELEOP)) ||
        ((jjogmode == JOGTELEOP_TOOL) && (zucStatus->motion.traj.mode != ZUC_TRAJ_MODE_TELEOP_TOOL)))
    {
        return;
    }

    if ((jjogmode == JOGJOINT) && (ja < 0 || ja >= num_joints))
    {
        rtapi_print("halui: unexpected_1 %d\n", ja);
        return;
    }
    if ((jjogmode != JOGJOINT) && (ja < 0))
    {
        rtapi_print("halui: unexpected_2 %d\n", ja);
        return;
    }
    if ((jjogmode != JOGJOINT) && !(axis_mask & (1 << ja)))
    {
        rtapi_print("halui: unexpected_3 %d\n", ja);
        return;
    }

    zuc_jog_stop_msg.jjogmode = jjogmode;
    zuc_jog_stop_msg.joint_or_axis = ja;
    zucCommandSend(zuc_jog_stop_msg);
}

static void sendJogCont(int ja, double speed, int jjogmode)
{
    ZUC_JOG_CONT zuc_jog_cont_msg;

    if (zucStatus->task.state != ZUC_TASK_STATE_ON)
    {
        return;
    }
    if (((jjogmode == JOGJOINT) && (zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_TELEOP || zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_TELEOP_TOOL)) ||
        ((jjogmode == JOGTELEOP) && (zucStatus->motion.traj.mode != ZUC_TRAJ_MODE_TELEOP)) ||
        ((jjogmode == JOGTELEOP_TOOL) && (zucStatus->motion.traj.mode != ZUC_TRAJ_MODE_TELEOP_TOOL)))
    {
        return;
    }

    if ((jjogmode == JOGJOINT) && (ja < 0 || ja >= num_joints))
    {
        rtapi_print("halui: unexpected_4 %d\n", ja);
        return;
    }
    if ((jjogmode != JOGJOINT) && (ja < 0))
    {
        rtapi_print("halui: unexpected_5 %d\n", ja);
        return;
    }
    if ((jjogmode != JOGJOINT) && !(axis_mask & (1 << ja)))
    {
        rtapi_print("halui: unexpected_6 %d\n", ja);
        return;
    }

    zuc_jog_cont_msg.jjogmode = jjogmode;
    zuc_jog_cont_msg.joint_or_axis = ja;
    zuc_jog_cont_msg.vel = speed / 60.0;

    zucCommandSend(zuc_jog_cont_msg);
}

static void sendJogIncr(int ja, double speed, double incr, int jjogmode)
{
    ZUC_JOG_INCR zuc_jog_incr_msg;

    if (zucStatus->task.state != ZUC_TASK_STATE_ON)
    {
        return;
    }
    if (((jjogmode == JOGJOINT) && (zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_TELEOP || zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_TELEOP_TOOL)) ||
        ((jjogmode == JOGTELEOP) && (zucStatus->motion.traj.mode != ZUC_TRAJ_MODE_TELEOP)) ||
        ((jjogmode == JOGTELEOP_TOOL) && (zucStatus->motion.traj.mode != ZUC_TRAJ_MODE_TELEOP_TOOL)))
    {
        return;
    }

    if ((jjogmode == JOGJOINT) && (ja < 0 || ja >= num_joints))
    {
        rtapi_print("halui: unexpected_7 %d\n", ja);
        return;
    }
    if ((jjogmode != JOGJOINT) && (ja < 0))
    {
        rtapi_print("halui: unexpected_8 %d\n", ja);
        return;
    }
    if ((jjogmode != JOGJOINT) && !(axis_mask & (1 << ja)))
    {
        rtapi_print("halui: unexpected_9 %d\n", ja);
        return;
    }

    zuc_jog_incr_msg.jjogmode = jjogmode;
    zuc_jog_incr_msg.joint_or_axis = ja;
    zuc_jog_incr_msg.vel = speed / 60.0;
    zuc_jog_incr_msg.incr = incr;

    zucCommandSend(zuc_jog_incr_msg);
}

static int sendFeedOverride(double override)
{
    ZUC_TRAJ_SET_SCALE zuc_traj_set_scale_msg;

    if (override < 0.0)
    {
        override = 0.0;
    }

    if (override > maxFeedOverride)
    {
        override = maxFeedOverride;
    }

    zuc_traj_set_scale_msg.scale = override;
    return zucCommandSend(zuc_traj_set_scale_msg);
}

static int sendRapidOverride(double override)
{
    ZUC_TRAJ_SET_RAPID_SCALE zuc_traj_set_scale_msg;

    if (override < 0.0)
    {
        override = 0.0;
    }

    if (override > 1.0)
    {
        override = 1.0;
    }

    zuc_traj_set_scale_msg.scale = override;
    return zucCommandSend(zuc_traj_set_scale_msg);
}

static int sendPercentageMode(int percentageLevel)
{
    ZUC_MOTION_PERCENTAGE_MODE zuc_mot_percentage_msg;
    zuc_mot_percentage_msg.percentage_level = percentageLevel;
    return zucCommandSend(zuc_mot_percentage_msg);
}

static int sendReduceMode(int reduce_mode)
{
    ZUC_MOTION_REDUCE_MODE zuc_mot_reduce_mode_msg;
    zuc_mot_reduce_mode_msg.reduce_mode = reduce_mode;
    return zucCommandSend(zuc_mot_reduce_mode_msg);
}

static int sendCollsionLevel(int level)
{
    ZUC_SET_SERVO_PARAM servo_param_msg;
    servo_param_msg.servoParam.paramType = ServoParam::CLSN_SENSITIVITY;
    servo_param_msg.servoParam.paramVal.clsn_sensitivity = level;
    return zucCommandSend(servo_param_msg);
}

static int setCustomizeType(int customize_type)
{
    ZUC_SET_ROBOT_SAFETY_CUSTOMIZE robot_safety_customize;
    robot_safety_customize.customize_type = customize_type;
    return zucCommandSend(robot_safety_customize);
}

static int sendCollisionDetectionEnable(int enable)
{
    ZUC_COLLISION_DETECTION_ENABLE collision_enable_msg;
    collision_enable_msg.collision_detection_enable = enable;
    return zucCommandSend(collision_enable_msg);
}

static int sendMaxVelocity(double velocity)
{
    ZUC_TRAJ_SET_MAX_VELOCITY mv;

    if (velocity < 0.0)
    {
        velocity = 0.0;
    }

    if (velocity > maxMaxVelocity)
    {
        velocity = maxMaxVelocity;
    }

    mv.velocity = velocity;
    return zucCommandSend(mv);
}

static int sendReturnTOInitialPosition(void)
{
    sendAuto();
    zucCommandWaitDone();
    ZUC_RETURN_TO_INITIAL_POSITION r;
    return zucCommandSend(r);
    // ZUC_TASK_ABORT task_abort_msg;

    // zucCommandSend(task_abort_msg);
    // ZUC_TRAJ_SET_TELEOP_ENABLE en1;
    // en1.enable = 0;
    // zucCommandSend(en1);
    // ZUC_RETURN_TO_INITIAL_POSITION r1;
    // return zucCommandSend(r1);
}

static int sendExitProtect(void)
{
    ZUC_MOTION_EXIT_PROTECT exit_prot;
    exit_prot.protect_type = 0;
    return zucCommandSend(exit_prot);
}

static int sendResetSafetyGuard(void)  // fall edge trigger
{
    ZUC_MOTION_EXIT_PROTECT exit_prot;
    exit_prot.protect_type = 1;
    return zucCommandSend(exit_prot);
}

static int sendShutDown()
{
    ZUC_SHUT_DOWN shutdown;
    return zucCommandSend(shutdown);
}

static int sendSpindleOverride(double override) { return 0; }

static int iniLoad(const char* filename)
{
    IniFile inifile;
    const char* inistring;
    double d;
    // int i;

    // open it
    if (inifile.Open(filename) == false)
    {
        return -1;
    }

    if (NULL != (inistring = inifile.Find("DEBUG", "ZUC")))
    {
        // copy to global
        if (1 != sscanf(inistring, "%i", &zuc_debug))
        {
            zuc_debug = 0;
        }
    }
    else
    {
        // not found, use default
        zuc_debug = 0;
    }

    if (inifile.Find(&maxMaxVelocity, "MAX_LINEAR_VELOCITY", "TRAJ") && inifile.Find(&maxMaxVelocity, "MAX_VELOCITY", "AXIS_X"))
        maxMaxVelocity = 1.0;

    if (NULL != (inistring = inifile.Find("MIN_SPINDLE_OVERRIDE", "DISPLAY")))
    {
        if (1 == sscanf(inistring, "%lf", &d) && d > 0.0)
        {
            minSpindleOverride = d;
        }
    }

    if (NULL != (inistring = inifile.Find("MAX_SPINDLE_OVERRIDE", "DISPLAY")))
    {
        if (1 == sscanf(inistring, "%lf", &d) && d > 0.0)
        {
            maxSpindleOverride = d;
        }
    }

    //inistring = inifile.Find("COORDINATES", "TRAJ");
    inistring = "X Y Z A B C";
    num_axes = 0;
    if (inistring)
    {
        if (strchr(inistring, 'x') || strchr(inistring, 'X'))
        {
            axis_mask |= 0x0001;
            num_axes++;
        }
        if (strchr(inistring, 'y') || strchr(inistring, 'Y'))
        {
            axis_mask |= 0x0002;
            num_axes++;
        }
        if (strchr(inistring, 'z') || strchr(inistring, 'Z'))
        {
            axis_mask |= 0x0004;
            num_axes++;
        }
        if (strchr(inistring, 'a') || strchr(inistring, 'A'))
        {
            axis_mask |= 0x0008;
            num_axes++;
        }
        if (strchr(inistring, 'b') || strchr(inistring, 'B'))
        {
            axis_mask |= 0x0010;
            num_axes++;
        }
        if (strchr(inistring, 'c') || strchr(inistring, 'C'))
        {
            axis_mask |= 0x0020;
            num_axes++;
        }
        if (strchr(inistring, 'u') || strchr(inistring, 'U'))
        {
            axis_mask |= 0x0040;
            num_axes++;
        }
        if (strchr(inistring, 'v') || strchr(inistring, 'V'))
        {
            axis_mask |= 0x0080;
            num_axes++;
        }
        if (strchr(inistring, 'w') || strchr(inistring, 'W'))
        {
            axis_mask |= 0x0100;
            num_axes++;
        }
    }
    if (num_axes == 0)
    {
        rcs_print("halui: no [TRAJ]COORDINATES specified, enabling all axes\n");
        num_axes = ZUCMOT_MAX_AXIS;
        axis_mask = 0xFFFF;
    }

    num_joints = NUM_JOINTS;

    if (NULL != (inistring = inifile.Find("LINEAR_UNITS", "DISPLAY")))
    {
        if (!strcmp(inistring, "AUTO"))
        {
            linearUnitConversion = LINEAR_UNITS_AUTO;
        }
        else if (!strcmp(inistring, "INCH"))
        {
            linearUnitConversion = LINEAR_UNITS_INCH;
        }
        else if (!strcmp(inistring, "MM"))
        {
            linearUnitConversion = LINEAR_UNITS_MM;
        }
        else if (!strcmp(inistring, "CM"))
        {
            linearUnitConversion = LINEAR_UNITS_CM;
        }
    }

    if (NULL != (inistring = inifile.Find("ANGULAR_UNITS", "DISPLAY")))
    {
        if (!strcmp(inistring, "AUTO"))
        {
            angularUnitConversion = ANGULAR_UNITS_AUTO;
        }
        else if (!strcmp(inistring, "DEG"))
        {
            angularUnitConversion = ANGULAR_UNITS_DEG;
        }
        else if (!strcmp(inistring, "RAD"))
        {
            angularUnitConversion = ANGULAR_UNITS_RAD;
        }
        else if (!strcmp(inistring, "GRAD"))
        {
            angularUnitConversion = ANGULAR_UNITS_GRAD;
        }
    }

    const char* mc;
    while (num_mdi_commands < MDI_MAX && (mc = inifile.Find("MDI_COMMAND", "HALUI", num_mdi_commands + 1))) { mdi_commands[num_mdi_commands++] = strdup(mc); }

    // close it
    inifile.Close();

    return 0;
}

static void hal_init_pins()
{
    int joint;
    int axis_num;

    *(halui_data->machine_on) = old_halui_data.machine_on = 0;
    *(halui_data->machine_off) = old_halui_data.machine_off = 0;
    *(halui_data->bp_enable) = old_halui_data.bp_enable = 0;

    *(halui_data->estop_activate) = old_halui_data.estop_activate = 0;
    *(halui_data->estop_reset) = old_halui_data.estop_reset = 0;
    *(halui_data->sys_shutdown) = old_halui_data.sys_shutdown = 0;

    for (joint = 0; joint < num_joints; joint++)
    {
        *(halui_data->joint_home[joint]) = old_halui_data.joint_home[joint] = 0;
        *(halui_data->joint_unhome[joint]) = old_halui_data.joint_unhome[joint] = 0;
        *(halui_data->joint_nr_select[joint]) = old_halui_data.joint_nr_select[joint] = 0;
        *(halui_data->jjog_minus[joint]) = old_halui_data.jjog_minus[joint] = 0;
        *(halui_data->jjog_plus[joint]) = old_halui_data.jjog_plus[joint] = 0;
        *(halui_data->jjog_analog[joint]) = old_halui_data.jjog_analog[joint] = 0;
        *(halui_data->jjog_increment[joint]) = old_halui_data.jjog_increment[joint] = 0.0;
        *(halui_data->jjog_increment_plus[joint]) = old_halui_data.jjog_increment_plus[joint] = 0;
        *(halui_data->jjog_increment_minus[joint]) = old_halui_data.jjog_increment_minus[joint] = 0;
    }

    for (axis_num = 0; axis_num < ZUCMOT_MAX_AXIS; axis_num++)
    {
        if (!(axis_mask & (1 << axis_num)))
        {
            continue;
        }
        *(halui_data->axis_nr_select[axis_num]) = old_halui_data.axis_nr_select[axis_num] = 0;
        *(halui_data->ajog_minus[axis_num]) = old_halui_data.ajog_minus[axis_num] = 0;
        *(halui_data->ajog_plus[axis_num]) = old_halui_data.ajog_plus[axis_num] = 0;
        *(halui_data->ajog_analog[axis_num]) = old_halui_data.ajog_analog[axis_num] = 0;
        *(halui_data->ajog_increment[axis_num]) = old_halui_data.ajog_increment[axis_num] = 0.0;
        *(halui_data->ajog_increment_plus[axis_num]) = old_halui_data.ajog_increment_plus[axis_num] = 0;
        *(halui_data->ajog_increment_minus[axis_num]) = old_halui_data.ajog_increment_minus[axis_num] = 0;
    }

    *(halui_data->joint_home[num_joints]) = old_halui_data.joint_home[num_joints] = 0;
    *(halui_data->jjog_minus[num_joints]) = old_halui_data.jjog_minus[num_joints] = 0;
    *(halui_data->jjog_plus[num_joints]) = old_halui_data.jjog_plus[num_joints] = 0;
    *(halui_data->jjog_increment[num_joints]) = old_halui_data.jjog_increment[num_joints] = 0.0;
    *(halui_data->jjog_increment_plus[num_joints]) = old_halui_data.jjog_increment_plus[num_joints] = 0;
    *(halui_data->jjog_increment_minus[num_joints]) = old_halui_data.jjog_increment_minus[num_joints] = 0;
    *(halui_data->jjog_deadband) = 0.2;
    *(halui_data->jjog_speed) = 0;
    *(halui_data->ajog_minus[ZUCMOT_MAX_AXIS]) = old_halui_data.ajog_minus[ZUCMOT_MAX_AXIS] = 0;
    *(halui_data->ajog_plus[ZUCMOT_MAX_AXIS]) = old_halui_data.ajog_plus[ZUCMOT_MAX_AXIS] = 0;
    *(halui_data->ajog_increment[ZUCMOT_MAX_AXIS]) = old_halui_data.ajog_increment[ZUCMOT_MAX_AXIS] = 0.0;
    *(halui_data->ajog_increment_plus[ZUCMOT_MAX_AXIS]) = old_halui_data.ajog_increment_plus[ZUCMOT_MAX_AXIS] = 0;
    *(halui_data->ajog_increment_minus[ZUCMOT_MAX_AXIS]) = old_halui_data.ajog_increment_minus[ZUCMOT_MAX_AXIS] = 0;
    *(halui_data->ajog_deadband) = 0.2;
    *(halui_data->ajog_speed) = 0;

    *(halui_data->joint_selected) = 0;  // select joint 0 by default
    *(halui_data->axis_selected) = 0;   // select axis 0 by default

    *(halui_data->fo_scale) = old_halui_data.fo_scale = 0.1;  //sane default
    *(halui_data->ro_scale) = old_halui_data.ro_scale = 0.1;  //sane default
    *(halui_data->so_scale) = old_halui_data.so_scale = 0.1;  //sane default
}

static int check_bit_changed(bool halpin, bool& newpin)
{
    if (halpin != newpin)
    {
        newpin = halpin;
        return halpin;
    }
    return 0;
}

static void check_other_mode(const char* from, bool new_percentage_mode_level2, bool new_percentage_mode_level1)
{
    char content[50];
    if (new_percentage_mode_level2)
    {
        strncpy(content, "[HalUI] send percentage mode level 2 from ", 50);
        strncat(content, from, 15);
        zucInnerDbg(content);
        sendPercentageMode(2);
    }
    else if (new_percentage_mode_level1)
    {
        strncpy(content, "[HalUI] send percentage mode level 1 from ", 50);
        strncat(content, from, 15);
        zucInnerDbg(content);
        sendPercentageMode(1);
    }
    else
    {
        strncpy(content, "[HalUI] send percentage mode reset from ", 50);
        strncat(content, from, 15);
        zucInnerDbg(content);
        /**
         * 如果上使能状态，且程序仍然在暂停状态，则恢复程序执行
         * 如果没有上使能状态下退出保护性停止，则不恢复程序执行，仅退出保护性停止
        */
        if (zucStatus->motion.is_enabled && zucStatus->task.interpState == ZUC_TASK_INTERP_PAUSED && zucStatus->motion.percentage_mode_level == 3)
        {
            sendProgramResume();
        }
        sendPercentageMode(0);
    }
}

static void copy_hal_data(const halui_str& i, local_halui_str& j)
{
    int x;
#define FIELD(t, f) j.f = (i.f) ? *i.f : 0;
#define ARRAY(t, f, n)                                           \
    do {                                                         \
        for (x = 0; x < n; x++) j.f[x] = (i.f[x]) ? *i.f[x] : 0; \
    } while (0);
    HAL_FIELDS
#undef FIELD
#undef ARRAY
}

// Returns true if any of halui.JA.N.plus, halui.JA.N.minus, or
// halui.JA.N.analog are true (where JA is joint or axis and
// N is the passed-in joint/axis number). Otherwise, returns false.
static bool jogging_joint(local_halui_str& hal, int joint) { return (hal.jjog_plus[joint] || hal.jjog_minus[joint] || hal.jjog_analog[joint]); }
static bool jogging_axis(local_halui_str& hal, int axis_num) { return (hal.ajog_plus[axis_num] || hal.ajog_minus[axis_num] || hal.ajog_analog[axis_num]); }

// Returns true if any of halui.JA.selected.plus,
// halui.JA.selected.minus, or halui.JA.selected.analog are true.
// JA == joint or axis as appropriate
// Otherwise, returns false.
static bool jogging_selected_joint(local_halui_str& hal) { return (hal.jjog_plus[num_joints] || hal.jjog_minus[num_joints]); }
static bool jogging_selected_axis(local_halui_str& hal) { return (hal.ajog_plus[ZUCMOT_MAX_AXIS] || hal.ajog_minus[ZUCMOT_MAX_AXIS]); }

// this function looks if any of the hal pins has changed
// and sends appropiate messages if so
static void check_hal_changes()
{
    hal_s32_t counts;
    int jselect_changed, joint;
    int aselect_changed, axis_num;
    hal_bit_t bit;
    int js;
    hal_float_t floatt;
    int jjog_speed_changed;
    int ajog_speed_changed;
    int safety_di_set_clsn_mask = 0;
    int safety_di_config_mask = 0;
    // static double saved_rapid_rate = 1.0;

    local_halui_str new_halui_data_mutable;
    copy_hal_data(*halui_data, new_halui_data_mutable);
    const local_halui_str& new_halui_data = new_halui_data_mutable;
    // get current program status, power status and servo status
    const int currInterState = zucStatus->task.interpState;

    if (check_bit_changed(new_halui_data.estop_activate, old_halui_data.estop_activate) != 0)
    {
        zucInnerDbg("[HalUI] send power off");
        sendEstop();
    }

    if (check_bit_changed(new_halui_data.estop_reset, old_halui_data.estop_reset) != 0)
    {
        // if (!new_halui_data.estop_activate)
        // {
        sendEstopReset();
        zucInnerDbg("[HalUI] send power on");
        // }
        // else
        // {
        //     zucInnerDbg("[HalUI] cannot power on when power-off is requested");
        // }
    }

    if (check_bit_changed(new_halui_data.machine_off, old_halui_data.machine_off) != 0)
    {
        zucInnerDbg("[HalUI] send servo disable");
        sendMachineOff();
    }

    if (check_bit_changed(new_halui_data.machine_on, old_halui_data.machine_on) != 0)
    {
        // if (!new_halui_data.estop_activate && !new_halui_data.machine_off)
        // {
        if (zucStatus->operation_mode_option == 1 && zucStatus->operation_mode == 0)
        {
            zucOperatorError(CANNOT_TRIGGER_AUTO_ENABLE_IN_MANUAL_MODE, " cannot trigger auto-enable in manual mode");
        }
        else
        {
            zucInnerDbg("[HalUI] send servo enable");
            sendMachineOn();
        }
        // }
        // else
        // {
        //     zucInnerDbg("[HalUI] cannot servo enable when power-off or servo-off is requested");
        // }
    }

    if (check_bit_changed(new_halui_data.bp_enable, old_halui_data.bp_enable) != 0)
    {
        if (zucStatus->motion.scb_stick_locked == 1)
        {
            zucInnerDbg("[HalUI] bpio send servo enable");
            sendMachineOn();
        }
        else
        {
            zucInnerDbg("[HalUI] trigger bpio failed, when control sourece is not stick");
        }
    }

    if (check_bit_changed(new_halui_data.mode_manual, old_halui_data.mode_manual) != 0)
        sendManual();

    if (check_bit_changed(new_halui_data.mode_auto, old_halui_data.mode_auto) != 0)
        sendAuto();

    if (check_bit_changed(new_halui_data.mode_mdi, old_halui_data.mode_mdi) != 0)
        sendMdi();

    // TIO key: enter or exit drag mode
    if (1 == new_halui_data.mode_drag && 0 == old_halui_data.mode_drag && zucStatus->motion.is_enabled)
    {
        //在taskmain 中检查及报错提示
        // if ((zucStatus->task.mode == ZUC_TASK_MODE_AUTO && currInterState == ZUC_TASK_INTERP_IDLE) || zucStatus->task.mode == ZUC_TASK_MODE_MANUAL)
        {
            sendDrag();
        }
    }
    else if (0 == new_halui_data.mode_drag && 1 == old_halui_data.mode_drag)
    {
        if (zucStatus->task.mode == ZUC_TASK_MODE_DRAG)
        {
            sendManual();
        }
    }
    check_bit_changed(new_halui_data.mode_drag, old_halui_data.mode_drag);

    // function DI: enter drag mode
    if (check_bit_changed(new_halui_data.mode_drag_entr, old_halui_data.mode_drag_entr) != 0 && zucStatus->motion.is_enabled)
    {
        //在taskmain 中检查及报错提示
        // if ((zucStatus->task.mode == ZUC_TASK_MODE_AUTO && currInterState == ZUC_TASK_INTERP_IDLE) || zucStatus->task.mode == ZUC_TASK_MODE_MANUAL)
        {
            sendDrag();
        }
    }

    // function DI: exit drag mode
    if (check_bit_changed(new_halui_data.mode_drag_exit, old_halui_data.mode_drag_exit) != 0)
    {
        if (zucStatus->task.mode == ZUC_TASK_MODE_DRAG)
        {
            sendManual();
        }
    }

    if (check_bit_changed(new_halui_data.mode_teleop, old_halui_data.mode_teleop) != 0)
    {
        sendTeleop();
    }

    if (check_bit_changed(new_halui_data.mode_teleop_tool, old_halui_data.mode_teleop_tool) != 0)
    {
        sendTeleop_tool();
    }

    if (check_bit_changed(new_halui_data.mode_joint, old_halui_data.mode_joint) != 0)
    {
        sendJoint();
    }

    if (check_bit_changed(new_halui_data.program_stop, old_halui_data.program_stop) != 0)
    {
        sendAbort();
        sendManual();
        zucInnerDbg("[HalUI] send program stop");
    }

    if (check_bit_changed(new_halui_data.program_run, old_halui_data.program_run) != 0 ||
        check_bit_changed(new_halui_data.program_auto_run, old_halui_data.program_auto_run) != 0)
    {
        int ret = -1;
        // printf("hal bit changed: %d, currInterState=%d\n", new_halui_data.program_run, currInterState);
        // if(!new_halui_data.program_stop && currInterState == ZUC_TASK_INTERP_IDLE)

        if (new_halui_data.program_stop || new_halui_data.program_pause)
        {
            zucOperatorError(ERR_FDI_PROGRUN_LOCKED, "start-program function is locked by sprogram_stop_req of funcIO.");
        }
        if (currInterState == ZUC_TASK_INTERP_IDLE)  //program_stop如果触发，  zuctaskmain 中会弹窗进行报错提示用户
        {
            zucInnerDbg("[HalUI] send program run request");
            //# TODO  目前的手自动模式为选择性配置，正式发布手自动模式时需修改
            if (zucStatus->operation_mode_option == 1)  // 开启了手自动功能，
            {
                if (zucStatus->operation_mode == 1)  // 当前为自动模式。
                {
                    if (zucStatus->task.autoWork.enable)  //设置了开机自动加载默认程序
                    {
                        std::vector<std::string> tmpVec = split(std::string(zucStatus->task.autoWork.filename), '/');
                        if (tmpVec.size() <= 6 || tmpVec.at(6).compare("none") == 0)  // 未配置默认程序
                            zucOperatorError(CONFIG_ERROR_AUTO_RUN_PROGRAM_IN_MANUAL_MODE, "wrong configuration of auto-run program in auto-mode");
                        else
                            ret = sendProgramOpen(zucStatus->task.autoWork.filename);  // //重新加载默认程序,加载失败会报错
                    }
                    else  // 未开启默认程序自动加载
                    {
                        zucOperatorError(CONFIG_ERROR_AUTO_RUN_PROGRAM_IN_MANUAL_MODE, "wrong configuration of auto-run program in auto-mode");
                    }
                    if (ret == 0)  // 加载完默认程序后，发送运行程序的指令
                    {
                        sendProgramRun(0);
                    }
                }
                else if (zucStatus->operation_mode == 0)  //当前为手动模式,禁止自动运行程序
                {
                    printf("The start program signal cannot be automatically triggered in manual mode\n");
                    zucOperatorError(CANNOT_TRIGGER_AUTO_RUN_PROGRAM_IN_MANUAL_MODE,
                                     "The start program signal cannot be automatically triggered in manual mode");
                }
            }
            else if (zucStatus->operation_mode_option == 0)  // 关闭了手自动功能
            {
                std::vector<std::string> tmpVec = split(std::string(zucStatus->task.autoWork.filename), '/');
                if (zucStatus->task.autoWork.enable)  // 如果配置了 默认程序自动加载;
                {
                    if (tmpVec.size() <= 6 || tmpVec.at(6).compare("none") == 0)  // 未配置默认程序
                    {
                        zucOperatorError(CONFIG_ERROR_AUTO_RUN_PROGRAM, "wrong configuration of auto-run program");
                    }
                    else
                    {
                        ret = sendProgramOpen(zucStatus->task.autoWork.filename);  // 重新加载默认程序,加载失败会报错
                        if (ret == 0)
                        {  // 加载完默认程序后，发送运行程序的指令
                            sendProgramRun(0);
                        }
                    }
                }
                else  // 没有配置 默认程序自动加载;
                {
                    if (tmpVec.size() <= 6 || tmpVec.at(6).compare("none") == 0)  // 未配置默认程序
                    {
                        //最近一次手动打开的程序不为空
                        if (zucStatus->task.file_manu_mode[0] != 0)
                        {
                            ret = sendProgramOpen(zucStatus->task.file_manu_mode);  // 重新加载最近打开的程序,加载失败会报错
                            if (ret == 0)                                           // 加载完后，发送运行程序的指令
                            {
                                printf("auto-run program: %s\n", zucStatus->task.file_manu_mode);
                                sendProgramRun(0);
                            }
                        }
                    }
                    else
                    {
                        //报错 请先配置默认程序，并开启“默认程序自动加载”
                        zucOperatorError(CONFIG_ERROR_AUTO_RUN_PROGRAM, "wrong configuration of auto-run program");
                    }
                }
            }
        }
        else
        {
            zucInnerDbg("[HalUI] cannot run program when program-stop is requested");
        }
    }

    if (check_bit_changed(new_halui_data.program_auto_run, old_halui_data.program_auto_run) != 0)
    {
        // printf("hal bit changed: %d, currInterState=%d\n", new_halui_data.program_run, currInterState);
        // if(!new_halui_data.program_stop && currInterState == ZUC_TASK_INTERP_IDLE)
        if (currInterState == ZUC_TASK_INTERP_IDLE)  //program_stop如果触发，  zuctaskmain 中会弹窗进行报错提示用户
        {
            zucInnerDbg("[HalUI] robot enabled, send program auto run request");
            sendProgramAutoRun(0);
        }
        else
        {
            zucInnerDbg("[HalUI] cannot run program when program-stop is requested");
        }
    }

    if (check_bit_changed(new_halui_data.program_pause, old_halui_data.program_pause) != 0)
    {
        if (currInterState == ZUC_TASK_INTERP_READING || currInterState == ZUC_TASK_INTERP_WAITING)
        {
            zucInnerDbg("[HalUI] send program pause request");
            sendProgramPause();
        }
    }

    if (check_bit_changed(new_halui_data.collision_program_pause, old_halui_data.collision_program_pause) != 0)
    {
        if (currInterState == ZUC_TASK_INTERP_READING || currInterState == ZUC_TASK_INTERP_WAITING)
        {
            zucInnerDbg("[HalUI] send collision program pause request");
            sendProgramPause();
        }
    }

    if (check_bit_changed(new_halui_data.program_resume, old_halui_data.program_resume) != 0)
    {
        // if(!new_halui_data.program_pause && (currInterState == ZUC_TASK_INTERP_PAUSED || zucStatus->task.task_paused)) {
        if ((currInterState == ZUC_TASK_INTERP_PAUSED || zucStatus->task.task_paused))
        {  //program_pause 如果触发，  zuctaskmain 中会弹窗进行报错提示用户
            zucInnerDbg("[HalUI] send program resume");
            sendProgramResume();
        }
        else
        {
            zucInnerDbg("[HalUI] cannot resume program due to restriction");
        }
    }

    if (check_bit_changed(new_halui_data.program_step, old_halui_data.program_step) != 0)
    {
        zucInnerDbg("[HalUI] send program step");
        sendProgramStep();
    }

    if (1 == new_halui_data.return_to_initial_position && 0 == old_halui_data.return_to_initial_position)
    {
        zucInnerDbg("[HalUI] send move back to initial position");
        sendReturnTOInitialPosition();
        printf("[ check_hal_changes ] : sending return to initial position \n");
    }
    else if (0 == new_halui_data.return_to_initial_position && 1 == old_halui_data.return_to_initial_position)
    {
        printf("[ check_hal_changes ] : detected   fall  \n");
        if (currInterState == ZUC_TASK_INTERP_IDLE)
        {
            printf("[ check_hal_changes ] : sending abort \n");
            sendAbort();
        }
    }
    check_bit_changed(new_halui_data.return_to_initial_position, old_halui_data.return_to_initial_position);

    /* clear error*/
    if (check_bit_changed(new_halui_data.clear_error, old_halui_data.clear_error) != 0)
    {
        zucInnerDbg("[HalUI] send exit protect command");
        sendExitProtect();
    }

    if (check_bit_changed(new_halui_data.sys_shutdown, old_halui_data.sys_shutdown) != 0)
    {
        zucInnerDbg("[HalUI] send shut down request");
        sendShutDown();
    }
    if (new_halui_data.reset_safetyguard == SAFETY_DI_ON && old_halui_data.reset_safetyguard == SAFETY_DI_OFF)
    {
        zucInnerDbg("[HalUI] send reset safety guard");
        sendResetSafetyGuard();
        // if (new_halui_data.safetyguard_mode == 1 || new_halui_data.stop_mode == 1)
        // {
        //     zucLogError(FAILED_RESET_PROTECTIVE_STOP, "cannot reset protective stop mode when DI still triggered\n");
        // }
        /*else*/
        if (new_halui_data.safetyguard_mode == 0 && new_halui_data.stop_mode == 0)
        {
            check_other_mode("stop", new_halui_data.percentage_mode_level2, new_halui_data.percentage_mode_level1);
        }
    }
    old_halui_data.reset_safetyguard = new_halui_data.reset_safetyguard;

    if (check_bit_changed(new_halui_data.program_os_on, old_halui_data.program_os_on) != 0)
    {
        sendSetOptionalStop(ON);
    }

    if (check_bit_changed(new_halui_data.program_os_off, old_halui_data.program_os_off) != 0)
        sendSetOptionalStop(OFF);

    if (check_bit_changed(new_halui_data.program_bd_on, old_halui_data.program_bd_on) != 0)
        sendSetBlockDelete(ON);

    if (check_bit_changed(new_halui_data.program_bd_off, old_halui_data.program_bd_off) != 0)
        sendSetBlockDelete(OFF);

    //功能DI防护停止模式 进入与退出 速度降低为0 优先级低于安全DI
    if (new_halui_data.safetyguard_mode == 0)
    {
        if (old_halui_data.stop_mode == 0 && new_halui_data.stop_mode != 0)
        {
            zucInnerDbg("[HalUI] Send safeguard stop (Function DI)");
            if (zucStatus->motion.is_enabled && (currInterState == ZUC_TASK_INTERP_READING || currInterState == ZUC_TASK_INTERP_WAITING))
            {
                sendProgramPause();
            }
            sendPercentageMode(3);
        }
        else if (old_halui_data.stop_mode != 0 && new_halui_data.stop_mode == 0)
        {
            if (zucStatus->motion.safetyguard_lock != 1)
            {
                //退出保护性停止模式,状态回退顺序:  二级倍率模式 → 一级倍率模式 → 退出
                check_other_mode("Function DI stop", new_halui_data.percentage_mode_level2, new_halui_data.percentage_mode_level1);
            }
        }
    }

    //安全DI保护性停止模式 进入与退出  速度降低为0
    if (old_halui_data.safetyguard_mode == 0 && new_halui_data.safetyguard_mode != 0)
    {
        zucInnerDbg("[HalUI] Send protective stop (Safety DI)");
        if (zucStatus->motion.is_enabled && (currInterState == ZUC_TASK_INTERP_READING || currInterState == ZUC_TASK_INTERP_WAITING))
        {
            sendProgramPause();
        }
        sendPercentageMode(3);
    }
    else if (old_halui_data.safetyguard_mode != 0 && new_halui_data.safetyguard_mode == 0)
    {
        if (zucStatus->motion.safetyguard_lock != 1)
        {
            //退出保护性停止模式,状态回退顺序:  二级倍率模式 → 一级倍率模式 → 退出
            check_other_mode("Safety DI stop", new_halui_data.percentage_mode_level2, new_halui_data.percentage_mode_level1);
        }
    }

    //二级倍率模式的进入与退出   优先级：stop> l2 > l1
    if (new_halui_data.stop_mode == 0 && new_halui_data.safetyguard_mode == 0)
    {
        //进入二级倍率模式
        if (old_halui_data.percentage_mode_level2 == 0 && new_halui_data.percentage_mode_level2 != 0)
        {
            zucInnerDbg("[HalUI] send percentage mode level 2");
            sendPercentageMode(2);
        }
        else if (old_halui_data.percentage_mode_level2 != 0 && new_halui_data.percentage_mode_level2 == 0)
        {
            check_other_mode("level 2", new_halui_data.percentage_mode_level2, new_halui_data.percentage_mode_level1);
        }
    }
    //一级倍率模式的进入与退出
    if (new_halui_data.stop_mode == 0 && new_halui_data.safetyguard_mode == 0 && new_halui_data.percentage_mode_level2 == 0)
    {
        if (old_halui_data.percentage_mode_level1 == 0 && new_halui_data.percentage_mode_level1 != 0)
        {
            zucInnerDbg("[HalUI] send percentage mode level 1");
            sendPercentageMode(1);
        }
        else if (old_halui_data.percentage_mode_level1 != 0 && new_halui_data.percentage_mode_level1 == 0)
        {
            check_other_mode("level 1", new_halui_data.percentage_mode_level2, new_halui_data.percentage_mode_level1);
        }
    }

    old_halui_data.percentage_mode_level1 = new_halui_data.percentage_mode_level1;
    old_halui_data.percentage_mode_level2 = new_halui_data.percentage_mode_level2;
    old_halui_data.stop_mode = new_halui_data.stop_mode;
    old_halui_data.safetyguard_mode = new_halui_data.safetyguard_mode;
    old_halui_data.ro_value = new_halui_data.ro_value;

    //缩减模式的进入与退出   上沿触发   下沿退出

    if (old_halui_data.reduce_mode == 0 && new_halui_data.reduce_mode != 0)
    {
        zucInnerDbg("[HalUI] send reduce mode");
        sendReduceMode(1);
    }
    else if (old_halui_data.reduce_mode != 0 && new_halui_data.reduce_mode == 0)
    {
        sendReduceMode(0);
    }
    old_halui_data.reduce_mode = new_halui_data.reduce_mode;

    //安全DI三位置使能  上沿退出三位置限制
    if (old_halui_data.three_position_enabling == 0 && new_halui_data.three_position_enabling != 0)
    {
        //处于手动模式 停止运动退出jog                     ||  自动模式的IDLE模式 停止运动退出mov                    ||  退出调试
        if (zucStatus->task.mode == ZUC_TASK_MODE_MANUAL || zucStatus->task.interpState == ZUC_TASK_INTERP_IDLE || zucStatus->stepStatus.isSteping == 1)
        {
            sendAbort();
            sendJoint();
            sendManual();
        }
    }
    old_halui_data.three_position_enabling = new_halui_data.three_position_enabling;

    //安全DI设置碰撞灵敏度
    if (new_halui_data.collision_lv1 == SAFETY_DI_ON && old_halui_data.collision_lv1 == SAFETY_DI_OFF)
    {
        zucInnerDbg("[HalUI] send change collision sensivity LV1(safe DI)");
        sendCollisionDetectionEnable(1);
        sendCollsionLevel(1);
        setCustomizeType(0);
    }
    else if (new_halui_data.collision_lv2 == SAFETY_DI_ON && old_halui_data.collision_lv2 == SAFETY_DI_OFF)
    {
        zucInnerDbg("[HalUI] send change collision sensivity LV2(safe DI)");
        sendCollisionDetectionEnable(1);
        sendCollsionLevel(2);
        setCustomizeType(0);
    }
    else if (new_halui_data.collision_lv3 == SAFETY_DI_ON && old_halui_data.collision_lv3 == SAFETY_DI_OFF)
    {
        zucInnerDbg("[HalUI] send change collision sensivity LV3(safe DI)");
        sendCollisionDetectionEnable(1);
        sendCollsionLevel(3);
        setCustomizeType(0);
    }
    else if (new_halui_data.collision_lv4 == SAFETY_DI_ON && old_halui_data.collision_lv4 == SAFETY_DI_OFF)
    {
        zucInnerDbg("[HalUI] send change collision sensivity LV4(safe DI)");
        sendCollisionDetectionEnable(1);
        sendCollsionLevel(4);
        setCustomizeType(0);
    }
    else if (new_halui_data.collision_lv5 == SAFETY_DI_ON && old_halui_data.collision_lv5 == SAFETY_DI_OFF)
    {
        zucInnerDbg("[HalUI] send change collision sensivity LV5(safe DI)");
        sendCollisionDetectionEnable(1);
        sendCollsionLevel(5);
        setCustomizeType(0);
    }
    else if (new_halui_data.collision_lv7 == SAFETY_DI_ON && old_halui_data.collision_lv7 == SAFETY_DI_OFF)
    {
        zucInnerDbg("[HalUI] send change collision sensivity LV7(safe DI)");
        sendCollisionDetectionEnable(1);  // 允许安全IO直接打开碰撞
        sendCollsionLevel(7);
        setCustomizeType(0);
    }
    else if (new_halui_data.collision_lv0 == SAFETY_DI_ON && old_halui_data.collision_lv0 == SAFETY_DI_OFF)
    {
        zucInnerDbg("[HalUI] send change collision sensivity LV0(safe DI)");
        sendCollisionDetectionEnable(0);  // 直接关闭碰撞
        sendCollsionLevel(0);
        setCustomizeType(0);
    }

    old_halui_data.collision_lv7 = new_halui_data.collision_lv7;
    old_halui_data.collision_lv5 = new_halui_data.collision_lv5;
    old_halui_data.collision_lv4 = new_halui_data.collision_lv4;
    old_halui_data.collision_lv3 = new_halui_data.collision_lv3;
    old_halui_data.collision_lv2 = new_halui_data.collision_lv2;
    old_halui_data.collision_lv1 = new_halui_data.collision_lv1;
    old_halui_data.collision_lv0 = new_halui_data.collision_lv0;

    //max-velocity stuff
    counts = new_halui_data.mv_counts;
    if (counts != old_halui_data.mv_counts)
    {
        if (new_halui_data.mv_count_enable)
        {
            if (new_halui_data.mv_direct_value)
            {
                sendMaxVelocity(counts * new_halui_data.mv_scale);
            }
            else
            {
                sendMaxVelocity(new_halui_data.mv_value + (counts - old_halui_data.mv_counts) * new_halui_data.mv_scale);
            }
        }
        old_halui_data.mv_counts = counts;
    }

    //feed-override stuff
    counts = new_halui_data.fo_counts;
    if (counts != old_halui_data.fo_counts)
    {
        if (new_halui_data.fo_count_enable)
        {
            if (new_halui_data.fo_direct_value)
            {
                sendFeedOverride(counts * new_halui_data.fo_scale);
            }
            else
            {
                sendFeedOverride(new_halui_data.fo_value + (counts - old_halui_data.fo_counts) * new_halui_data.fo_scale);
            }
        }
        old_halui_data.fo_counts = counts;
    }

    //rapid-override stuff
    counts = new_halui_data.ro_counts;
    if (counts != old_halui_data.ro_counts)
    {
        if (new_halui_data.ro_count_enable)
        {
            if (new_halui_data.ro_direct_value)
            {
                sendRapidOverride(counts * new_halui_data.ro_scale);
            }
            else
            {
                sendRapidOverride(new_halui_data.ro_value + (counts - old_halui_data.ro_counts) * new_halui_data.ro_scale);
            }
        }
        old_halui_data.ro_counts = counts;
    }

    //spindle-override stuff
    counts = new_halui_data.so_counts;
    if (counts != old_halui_data.so_counts)
    {
        if (new_halui_data.so_count_enable)
        {
            if (new_halui_data.so_direct_value)
            {
                sendSpindleOverride(counts * new_halui_data.so_scale);
            }
            else
            {
                sendSpindleOverride(new_halui_data.so_value + (counts - old_halui_data.so_counts) * new_halui_data.so_scale);
            }
        }
        old_halui_data.so_counts = counts;
    }

    if (check_bit_changed(new_halui_data.mv_increase, old_halui_data.mv_increase) != 0)
        sendMaxVelocity(new_halui_data.mv_value + new_halui_data.mv_scale);
    if (check_bit_changed(new_halui_data.mv_decrease, old_halui_data.mv_decrease) != 0)
        sendMaxVelocity(new_halui_data.mv_value - new_halui_data.mv_scale);

    if (check_bit_changed(new_halui_data.fo_increase, old_halui_data.fo_increase) != 0)
        sendFeedOverride(new_halui_data.fo_value + new_halui_data.fo_scale);
    if (check_bit_changed(new_halui_data.fo_decrease, old_halui_data.fo_decrease) != 0)
        sendFeedOverride(new_halui_data.fo_value - new_halui_data.fo_scale);

    if (check_bit_changed(new_halui_data.ro_increase, old_halui_data.ro_increase) != 0)
        sendRapidOverride(new_halui_data.ro_value + new_halui_data.ro_scale);
    if (check_bit_changed(new_halui_data.ro_decrease, old_halui_data.ro_decrease) != 0)
        sendRapidOverride(new_halui_data.ro_value - new_halui_data.ro_scale);

    if (check_bit_changed(new_halui_data.so_increase, old_halui_data.so_increase) != 0)
        sendSpindleOverride(new_halui_data.so_value + new_halui_data.so_scale);
    if (check_bit_changed(new_halui_data.so_decrease, old_halui_data.so_decrease) != 0)
        sendSpindleOverride(new_halui_data.so_value - new_halui_data.so_scale);

    if (check_bit_changed(new_halui_data.abort, old_halui_data.abort) != 0)
        sendAbort();

    if (check_bit_changed(new_halui_data.home_all, old_halui_data.home_all) != 0)
    {
        if (!zucStatus->motion.joint[0].homed)
            sendHome(-1);
    }

    // joint stuff (selection, homing..)
    jselect_changed = -1;  // flag to see if the selected joint changed

    // if the jog-speed changes while in a continuous jog, we want to
    // re-start the jog with the new speed
    if (fabs(old_halui_data.jjog_speed - new_halui_data.jjog_speed) > 0.00001)
    {
        old_halui_data.jjog_speed = new_halui_data.jjog_speed;
        jjog_speed_changed = 1;
    }
    else
    {
        jjog_speed_changed = 0;
    }
    // axis stuff (selection, homing..)
    aselect_changed = -1;  // flag to see if the selected joint changed

    // if the jog-speed changes while in a continuous jog, we want to
    // re-start the jog with the new speed
    if (fabs(old_halui_data.ajog_speed - new_halui_data.ajog_speed) > 0.00001)
    {
        old_halui_data.ajog_speed = new_halui_data.ajog_speed;
        ajog_speed_changed = 1;
    }
    else
    {
        ajog_speed_changed = 0;
    }

    for (joint = 0; joint < num_joints; joint++)
    {
        if (check_bit_changed(new_halui_data.joint_home[joint], old_halui_data.joint_home[joint]) != 0)
            sendHome(joint);

        if (check_bit_changed(new_halui_data.joint_unhome[joint], old_halui_data.joint_unhome[joint]) != 0)
            sendUnhome(joint);

        bit = new_halui_data.jjog_minus[joint];
        if ((bit != old_halui_data.jjog_minus[joint]) || (bit && jjog_speed_changed))
        {
            if (bit != 0)
                sendJogCont(joint, -new_halui_data.jjog_speed, JOGJOINT);
            else
                sendJogStop(joint, JOGJOINT);
            old_halui_data.jjog_minus[joint] = bit;
        }

        bit = new_halui_data.jjog_plus[joint];
        if ((bit != old_halui_data.jjog_plus[joint]) || (bit && jjog_speed_changed))
        {
            if (bit != 0)
                sendJogCont(joint, new_halui_data.jjog_speed, JOGJOINT);
            else
                sendJogStop(joint, JOGJOINT);
            old_halui_data.jjog_plus[joint] = bit;
        }

        floatt = new_halui_data.jjog_analog[joint];
        bit = (fabs(floatt) > new_halui_data.jjog_deadband);
        if ((floatt != old_halui_data.jjog_analog[joint]) || (bit && jjog_speed_changed))
        {
            if (bit)
                sendJogCont(joint, (new_halui_data.jjog_speed) * (new_halui_data.jjog_analog[joint]), JOGJOINT);
            else
                sendJogStop(joint, JOGJOINT);
            old_halui_data.jjog_analog[joint] = floatt;
        }

        bit = new_halui_data.jjog_increment_plus[joint];
        if (bit != old_halui_data.jjog_increment_plus[joint])
        {
            if (bit)
                sendJogIncr(joint, new_halui_data.jjog_speed, new_halui_data.jjog_increment[joint], JOGJOINT);
            old_halui_data.jjog_increment_plus[joint] = bit;
        }

        bit = new_halui_data.jjog_increment_minus[joint];
        if (bit != old_halui_data.jjog_increment_minus[joint])
        {
            if (bit)
                sendJogIncr(joint, new_halui_data.jjog_speed, -(new_halui_data.jjog_increment[joint]), JOGJOINT);
            old_halui_data.jjog_increment_minus[joint] = bit;
        }

        // check to see if another joint has been selected
        bit = new_halui_data.joint_nr_select[joint];
        if (bit != old_halui_data.joint_nr_select[joint])
        {
            if (bit != 0)
            {
                *halui_data->joint_selected = joint;
                jselect_changed = joint;  // flag that we changed the selected joint
            }
            old_halui_data.joint_nr_select[joint] = bit;
        }
    }

    if (jselect_changed >= 0)
    {
        for (joint = 0; joint < num_joints; joint++)
        {
            if (joint != jselect_changed)
            {
                *(halui_data->joint_is_selected[joint]) = 0;
                if (jogging_selected_joint(old_halui_data) && !jogging_joint(old_halui_data, joint))
                {
                    sendJogStop(joint, JOGJOINT);
                }
            }
            else
            {
                *(halui_data->joint_is_selected[joint]) = 1;
                if (*halui_data->jjog_plus[num_joints])
                {
                    sendJogCont(joint, new_halui_data.jjog_speed, JOGJOINT);
                }
                else if (*halui_data->jjog_minus[num_joints])
                {
                    sendJogCont(joint, -new_halui_data.jjog_speed, JOGJOINT);
                }
            }
        }
    }

    for (axis_num = 0; axis_num < ZUCMOT_MAX_AXIS; axis_num++)
    {
        if (!(axis_mask & (1 << axis_num)))
        {
            continue;
        }
        bit = new_halui_data.ajog_minus[axis_num];
        if ((bit != old_halui_data.ajog_minus[axis_num]) || (bit && ajog_speed_changed))
        {
            if (bit != 0)
                sendJogCont(axis_num, -new_halui_data.ajog_speed, JOGTELEOP);
            else
                sendJogStop(axis_num, JOGTELEOP);
            old_halui_data.ajog_minus[axis_num] = bit;
        }

        bit = new_halui_data.ajog_plus[axis_num];
        if ((bit != old_halui_data.ajog_plus[axis_num]) || (bit && ajog_speed_changed))
        {
            if (bit != 0)
                sendJogCont(axis_num, new_halui_data.ajog_speed, JOGTELEOP);
            else
                sendJogStop(axis_num, JOGTELEOP);
            old_halui_data.ajog_plus[axis_num] = bit;
        }

        floatt = new_halui_data.ajog_analog[axis_num];
        bit = (fabs(floatt) > new_halui_data.ajog_deadband);
        if ((floatt != old_halui_data.ajog_analog[axis_num]) || (bit && ajog_speed_changed))
        {
            if (bit)
                sendJogCont(axis_num, (new_halui_data.ajog_speed) * (new_halui_data.ajog_analog[axis_num]), JOGTELEOP);
            else
                sendJogStop(axis_num, JOGTELEOP);
            old_halui_data.ajog_analog[axis_num] = floatt;
        }

        bit = new_halui_data.ajog_increment_plus[axis_num];
        if (bit != old_halui_data.ajog_increment_plus[axis_num])
        {
            if (bit)
                sendJogIncr(axis_num, new_halui_data.ajog_speed, new_halui_data.ajog_increment[axis_num], JOGTELEOP);
            old_halui_data.ajog_increment_plus[axis_num] = bit;
        }

        bit = new_halui_data.ajog_increment_minus[axis_num];
        if (bit != old_halui_data.ajog_increment_minus[axis_num])
        {
            if (bit)
                sendJogIncr(axis_num, new_halui_data.ajog_speed, -(new_halui_data.ajog_increment[axis_num]), JOGTELEOP);
            old_halui_data.ajog_increment_minus[axis_num] = bit;
        }

        // check to see if another axis has been selected
        bit = new_halui_data.axis_nr_select[axis_num];
        if (bit != old_halui_data.axis_nr_select[axis_num])
        {
            if (bit != 0)
            {
                *halui_data->axis_selected = axis_num;
                aselect_changed = axis_num;  // flag that we changed the selected axis
            }
            old_halui_data.axis_nr_select[axis_num] = bit;
        }
    }

    if (aselect_changed >= 0)
    {
        for (axis_num = 0; axis_num < ZUCMOT_MAX_AXIS; axis_num++)
        {
            if (!(axis_mask & (1 << axis_num)))
            {
                continue;
            }
            if (axis_num != aselect_changed)
            {
                *(halui_data->axis_is_selected[axis_num]) = 0;
                if (jogging_selected_axis(old_halui_data) && !jogging_axis(old_halui_data, axis_num))
                {
                    sendJogStop(axis_num, JOGTELEOP);
                }
            }
            else
            {
                *(halui_data->axis_is_selected[axis_num]) = 1;
                if (*halui_data->ajog_plus[num_axes])
                {
                    sendJogCont(axis_num, new_halui_data.ajog_speed, JOGTELEOP);
                }
                else if (*halui_data->ajog_minus[num_axes])
                {
                    sendJogCont(axis_num, -new_halui_data.ajog_speed, JOGTELEOP);
                }
            }
        }
    }

    if (check_bit_changed(new_halui_data.joint_home[num_joints], old_halui_data.joint_home[num_joints]) != 0)
        sendHome(new_halui_data.joint_selected);

    if (check_bit_changed(new_halui_data.joint_unhome[num_joints], old_halui_data.joint_unhome[num_joints]) != 0)
        sendUnhome(new_halui_data.joint_selected);

    bit = new_halui_data.jjog_minus[num_joints];
    js = new_halui_data.joint_selected;
    if ((bit != old_halui_data.jjog_minus[num_joints]) || (bit && jjog_speed_changed))
    {
        if (bit != 0)
            sendJogCont(js, -new_halui_data.jjog_speed, JOGJOINT);
        else
            sendJogStop(js, JOGJOINT);
        old_halui_data.jjog_minus[num_joints] = bit;
    }

    bit = new_halui_data.jjog_plus[num_joints];
    js = new_halui_data.joint_selected;
    if ((bit != old_halui_data.jjog_plus[num_joints]) || (bit && jjog_speed_changed))
    {
        if (bit != 0)
            sendJogCont(js, new_halui_data.jjog_speed, JOGJOINT);
        else
            sendJogStop(js, JOGJOINT);
        old_halui_data.jjog_plus[num_joints] = bit;
    }

    bit = new_halui_data.jjog_increment_plus[num_joints];
    js = new_halui_data.joint_selected;
    if (bit != old_halui_data.jjog_increment_plus[num_joints])
    {
        if (bit)
            sendJogIncr(js, new_halui_data.jjog_speed, new_halui_data.jjog_increment[num_joints], JOGJOINT);
        old_halui_data.jjog_increment_plus[num_joints] = bit;
    }

    bit = new_halui_data.jjog_increment_minus[num_joints];
    js = new_halui_data.joint_selected;
    if (bit != old_halui_data.jjog_increment_minus[num_joints])
    {
        if (bit)
            sendJogIncr(js, new_halui_data.jjog_speed, -(new_halui_data.jjog_increment[num_joints]), JOGJOINT);
        old_halui_data.jjog_increment_minus[num_joints] = bit;
    }

    bit = new_halui_data.ajog_minus[ZUCMOT_MAX_AXIS];
    js = new_halui_data.axis_selected;
    if ((bit != old_halui_data.ajog_minus[ZUCMOT_MAX_AXIS]) || (bit && ajog_speed_changed))
    {
        if (bit != 0)
            sendJogCont(js, -new_halui_data.ajog_speed, JOGTELEOP);
        else
            sendJogStop(js, JOGTELEOP);
        old_halui_data.ajog_minus[ZUCMOT_MAX_AXIS] = bit;
    }

    bit = new_halui_data.ajog_plus[ZUCMOT_MAX_AXIS];
    js = new_halui_data.axis_selected;
    if ((bit != old_halui_data.ajog_plus[ZUCMOT_MAX_AXIS]) || (bit && ajog_speed_changed))
    {
        if (bit != 0)
            sendJogCont(js, new_halui_data.ajog_speed, JOGTELEOP);
        else
            sendJogStop(js, JOGTELEOP);
        old_halui_data.ajog_plus[ZUCMOT_MAX_AXIS] = bit;
    }

    bit = new_halui_data.ajog_increment_plus[ZUCMOT_MAX_AXIS];
    js = new_halui_data.axis_selected;
    if (bit != old_halui_data.ajog_increment_plus[ZUCMOT_MAX_AXIS])
    {
        if (bit)
            sendJogIncr(js, new_halui_data.ajog_speed, new_halui_data.ajog_increment[ZUCMOT_MAX_AXIS], JOGTELEOP);
        old_halui_data.ajog_increment_plus[ZUCMOT_MAX_AXIS] = bit;
    }

    bit = new_halui_data.ajog_increment_minus[ZUCMOT_MAX_AXIS];
    js = new_halui_data.axis_selected;
    if (bit != old_halui_data.ajog_increment_minus[ZUCMOT_MAX_AXIS])
    {
        if (bit)
            sendJogIncr(js, new_halui_data.ajog_speed, -(new_halui_data.ajog_increment[ZUCMOT_MAX_AXIS]), JOGTELEOP);
        old_halui_data.ajog_increment_minus[ZUCMOT_MAX_AXIS] = bit;
    }

    for (int n = 0; n < num_mdi_commands; n++)
    {
        if (check_bit_changed(new_halui_data.mdi_commands[n], old_halui_data.mdi_commands[n]) != 0)
            sendMdiCommand(n);
    }
}

// @FIXME 7轴 适配
static int check_robot_is_moving()
{
    int joint = 0;
    int moving = 0;
    int joint_val = 0;
    static double last_joint_pos_fb[6];
    for (joint = 0; joint < 6; joint++)
    {
        joint_val = joint_val + fabs(zucStatus->motion.joint[joint].velocity);
        moving = moving || (fabs(zucStatus->motion.joint[joint].input - last_joint_pos_fb[joint]) > 0.01);
        last_joint_pos_fb[joint] = zucStatus->motion.joint[joint].input;
    }

    //拖拽模式使用反馈值判定运动状态
    if (zucStatus->task.mode == ZUC_TASK_MODE_DRAG && moving)
    {
        return 1;
    }
    //非拖拽模式使用指令值判定运动状态（避免伺服反馈值有噪音导致的DO闪烁）
    if (joint_val > 1.25)
    {
        return 1;
    }
    return 0;
}

// this function looks at the received NML status message
// and modifies the appropiate HAL pins
static void modify_hal_pins()
{
    int joint;

    if (zucStatus->task.state == ZUC_TASK_STATE_ON)
    {
        *(halui_data->machine_is_on) = 1;
    }
    else
    {
        *(halui_data->machine_is_on) = 0;
    }

    if (zucStatus->task.state == ZUC_TASK_STATE_POWERED_OFF)
    {
        *(halui_data->estop_is_activated) = 1;
    }
    else
    {
        *(halui_data->estop_is_activated) = 0;
    }

    if (halui_sent_mdi)
    {  // we have an ongoing MDI command
        if (zucStatus->status == 1)
        {  //which seems to have finished
            halui_sent_mdi = 0;
            switch (halui_old_mode)
            {
            case ZUC_TASK_MODE_MANUAL:
                sendManual();
                break;
            case ZUC_TASK_MODE_MDI:
                break;
            case ZUC_TASK_MODE_AUTO:
                sendAuto();
                break;
            default:
                sendManual();
                break;
            }
        }
    }

    if (zucStatus->task.mode == ZUC_TASK_MODE_MANUAL)
    {
        *(halui_data->mode_is_manual) = 1;
    }
    else
    {
        *(halui_data->mode_is_manual) = 0;
    }

    if (zucStatus->task.mode == ZUC_TASK_MODE_AUTO)
    {
        *(halui_data->mode_is_auto) = 1;
    }
    else
    {
        *(halui_data->mode_is_auto) = 0;
    }

    if (zucStatus->task.mode == ZUC_TASK_MODE_MDI)
    {
        *(halui_data->mode_is_mdi) = 1;
    }
    else
    {
        *(halui_data->mode_is_mdi) = 0;
    }

    if (zucStatus->task.mode == ZUC_TASK_MODE_DRAG)
    {
        *(halui_data->mode_is_drag) = 1;
    }
    else
    {
        *(halui_data->mode_is_drag) = 0;
    }

    if (zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_TELEOP)
    {
        *(halui_data->mode_is_teleop) = 1;
    }
    else
    {
        *(halui_data->mode_is_teleop) = 0;
    }

    /*may need add handling for ZUC_TRAJ_MODE_TELEOP_TOOL*/
    if (zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_TELEOP_TOOL)
    {
        *(halui_data->mode_is_teleop_tool) = 1;
    }
    else
    {
        *(halui_data->mode_is_teleop_tool) = 0;
    }

    if (zucStatus->motion.traj.mode == ZUC_TRAJ_MODE_FREE)
    {
        *(halui_data->mode_is_joint) = 1;
    }
    else
    {
        *(halui_data->mode_is_joint) = 0;
    }

    *(halui_data->robot_is_poweron) = zucStatus->motion.powered_on;
    *(halui_data->robot_is_enabled) = zucStatus->motion.is_enabled;

    //运动状态与停止状态功能DO使用私服反馈值进行判定
    if (!check_robot_is_moving())
    {
        *(halui_data->robot_is_static) = 1;
        *(halui_data->robot_is_motion) = 0;
    }
    else
    {
        *(halui_data->robot_is_motion) = 1;
        *(halui_data->robot_is_static) = 0;
    }
    //XXX+lin
    *(halui_data->robot_is_emergency_stop) = zucStatus->motion.emergency_stop;
    *(halui_data->robot_in_percentage_mode_level2) = (zucStatus->motion.percentage_mode_level == 2) ? 1 : 0;
    *(halui_data->robot_in_percentage_mode_level1) = (zucStatus->motion.percentage_mode_level == 1) ? 1 : 0;
    *(halui_data->robot_in_sys_protective_stop) = (zucStatus->motion.percentage_mode_level == 3) ? 1 : 0;

    *(halui_data->robot_is_boot) = 1;

    *(halui_data->program_is_paused) = (zucStatus->task.interpState == ZUC_TASK_INTERP_PAUSED || zucStatus->task.task_paused);
    *(halui_data->program_is_running) =
        (zucStatus->task.interpState == ZUC_TASK_INTERP_READING || zucStatus->task.interpState == ZUC_TASK_INTERP_WAITING) && (!zucStatus->task.task_paused);
    *(halui_data->stepping_is_running) = (zucStatus->steppingState == ZUC_TASK_INTERP_READING);
    *(halui_data->program_is_idle) = zucStatus->task.interpState == ZUC_TASK_INTERP_IDLE;
    *(halui_data->program_os_is_on) = zucStatus->task.optional_stop_state;
    *(halui_data->program_bd_is_on) = zucStatus->task.block_delete_state;

    *(halui_data->mv_value) = zucStatus->motion.traj.maxVelocity;
    *(halui_data->fo_value) = zucStatus->motion.traj.scale;        //feedoverride from 0 to 1 for 100%
    *(halui_data->ro_value) = zucStatus->motion.traj.rapid_scale;  //rapid override from 0 to 1 for 100%

    // printf("*(halui_data->ro_value) = %d\n", *(halui_data->ro_value));

    for (joint = 0; joint < num_joints; joint++)
    {
        *(halui_data->joint_is_homed[joint]) = zucStatus->motion.joint[joint].homed;
        *(halui_data->joint_on_soft_min_limit[joint]) = zucStatus->motion.joint[joint].minSoftLimit;
        *(halui_data->joint_on_soft_max_limit[joint]) = zucStatus->motion.joint[joint].maxSoftLimit;
        *(halui_data->joint_on_hard_min_limit[joint]) = zucStatus->motion.joint[joint].minHardLimit;

        *(halui_data->joint_on_hard_max_limit[joint]) = zucStatus->motion.joint[joint].maxHardLimit;
        *(halui_data->joint_has_fault[joint]) = zucStatus->motion.joint[joint].fault;
    }

    if (axis_mask & 0x0001)
    {
        *(halui_data->axis_pos_commanded[0]) = zucStatus->motion.traj.position.tran.x;
        *(halui_data->axis_pos_feedback[0]) = zucStatus->motion.traj.actualPosition.tran.x;
        *(halui_data->axis_pos_relative[0]) = zucStatus->motion.traj.actualPosition.tran.x - zucStatus->task.robotToolOffset.tran.x;
    }

    if (axis_mask & 0x0002)
    {
        *(halui_data->axis_pos_commanded[1]) = zucStatus->motion.traj.position.tran.y;
        *(halui_data->axis_pos_feedback[1]) = zucStatus->motion.traj.actualPosition.tran.y;
        *(halui_data->axis_pos_relative[1]) = zucStatus->motion.traj.actualPosition.tran.y - zucStatus->task.robotToolOffset.tran.y;
    }

    if (axis_mask & 0x0004)
    {
        *(halui_data->axis_pos_commanded[2]) = zucStatus->motion.traj.position.tran.z;
        *(halui_data->axis_pos_feedback[2]) = zucStatus->motion.traj.actualPosition.tran.z;
        *(halui_data->axis_pos_relative[2]) = zucStatus->motion.traj.actualPosition.tran.z - zucStatus->task.robotToolOffset.tran.z;
    }

    if (axis_mask & 0x0008)
    {
        *(halui_data->axis_pos_commanded[3]) = zucStatus->motion.traj.position.a;
        *(halui_data->axis_pos_feedback[3]) = zucStatus->motion.traj.actualPosition.a;
        *(halui_data->axis_pos_relative[3]) = zucStatus->motion.traj.actualPosition.a - zucStatus->task.robotToolOffset.a;
    }

    if (axis_mask & 0x0010)
    {
        *(halui_data->axis_pos_commanded[4]) = zucStatus->motion.traj.position.b;
        *(halui_data->axis_pos_feedback[4]) = zucStatus->motion.traj.actualPosition.b;
        *(halui_data->axis_pos_relative[4]) = zucStatus->motion.traj.actualPosition.b - zucStatus->task.robotToolOffset.b;
    }

    if (axis_mask & 0x0020)
    {
        *(halui_data->axis_pos_commanded[5]) = zucStatus->motion.traj.position.c;
        *(halui_data->axis_pos_feedback[5]) = zucStatus->motion.traj.actualPosition.c;
        *(halui_data->axis_pos_relative[5]) = zucStatus->motion.traj.actualPosition.c - zucStatus->task.robotToolOffset.c;
    }

    if (axis_mask & 0x0040)
    {
        *(halui_data->axis_pos_commanded[6]) = zucStatus->motion.traj.position.u;
        *(halui_data->axis_pos_feedback[6]) = zucStatus->motion.traj.actualPosition.u;
        *(halui_data->axis_pos_relative[6]) = zucStatus->motion.traj.actualPosition.u - zucStatus->task.robotToolOffset.u;
    }

    if (axis_mask & 0x0080)
    {
        *(halui_data->axis_pos_commanded[7]) = zucStatus->motion.traj.position.v;
        *(halui_data->axis_pos_feedback[7]) = zucStatus->motion.traj.actualPosition.v;
        *(halui_data->axis_pos_relative[7]) = zucStatus->motion.traj.actualPosition.v - zucStatus->task.robotToolOffset.v;
    }

    if (axis_mask & 0x0100)
    {
        *(halui_data->axis_pos_commanded[8]) = zucStatus->motion.traj.position.w;
        *(halui_data->axis_pos_feedback[8]) = zucStatus->motion.traj.actualPosition.w;
        *(halui_data->axis_pos_relative[8]) = zucStatus->motion.traj.actualPosition.w - zucStatus->task.robotToolOffset.w;
    }

    *(halui_data->joint_is_homed[num_joints]) = zucStatus->motion.joint[*(halui_data->joint_selected)].homed;
    *(halui_data->joint_on_soft_min_limit[num_joints]) = zucStatus->motion.joint[*(halui_data->joint_selected)].minSoftLimit;
    *(halui_data->joint_on_soft_max_limit[num_joints]) = zucStatus->motion.joint[*(halui_data->joint_selected)].maxSoftLimit;
    *(halui_data->joint_on_hard_min_limit[num_joints]) = zucStatus->motion.joint[*(halui_data->joint_selected)].minHardLimit;
    *(halui_data->joint_on_hard_max_limit[num_joints]) = zucStatus->motion.joint[*(halui_data->joint_selected)].maxHardLimit;
    *(halui_data->joint_has_fault[num_joints]) = zucStatus->motion.joint[*(halui_data->joint_selected)].fault;
}

int main(int argc, char* argv[])
{
    // process command line args
    if (0 != zucGetArgs(argc, argv))
    {
        rcs_print_error("error in argument list\n");
        exit(1);
    }

    // get configuration information
    if (0 != iniLoad(zuc_inifile))
    {
        rcs_print_error("iniLoad error\n");
        exit(2);
    }

    //init HAL and export pins
    if (0 != halui_hal_init())
    {
        rcs_print_error("hal_init error\n");
        exit(1);
    }

    //initialize safe values
    hal_init_pins();

    // init NML
    if (0 != tryNml())
    {
        rcs_print_error("can't connect to zuc\n");
        thisQuit();
        exit(1);
    }

    // get current serial number, and save it for restoring when we quit
    // so as not to interfere with real operator interface
    updateStatus();

    done = 0;
    /* Register the routine that catches the SIGINT signal */
    // signal(SIGINT, quit);
    /* catch SIGTERM too - the run script uses it to shut things down */
    // signal(SIGTERM, quit);

    while (!done)
    {
        check_hal_changes();  //if anything changed send NML messages

        modify_hal_pins();  //if status changed modify HAL too

        esleep(0.02);  //sleep for a while

        updateStatus();
    }
    thisQuit();
    return 0;
}
